From 6ef8cc91ffe703c10537d12c9f9b6556f59fa9de Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Thu, 28 Sep 2023 15:29:54 +0200 Subject: [PATCH 01/42] feat: added cppparser && make the project compile on my end --- .clang-format | 1 + .gitmodules | 6 + .pre-commit-config.yaml | 2 +- CMakeLists.txt | 29 +- CppParser | 1 + common | 1 + .../hardware_interface/hardware_interface.hpp | 26 +- src/ros2_control_py/core.cpp | 9 +- .../hardware_interface/hardware_interface.cpp | 286 ++++++++---------- 9 files changed, 178 insertions(+), 183 deletions(-) create mode 100644 .clang-format create mode 100644 .gitmodules create mode 160000 CppParser create mode 160000 common rename {src/ros2_control_py => include}/hardware_interface/hardware_interface.hpp (74%) diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..f6cb8ad --- /dev/null +++ b/.clang-format @@ -0,0 +1 @@ +BasedOnStyle: Google diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..5b69abf --- /dev/null +++ b/.gitmodules @@ -0,0 +1,6 @@ +[submodule "common"] + path = common + url = https://github.com/satya-das/common.git +[submodule "CppParser"] + path = CppParser + url = https://github.com/satya-das/CppParser.git diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 85a52a3..404cdb5 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -45,7 +45,7 @@ repos: entry: clang-format language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-fallback-style=none', '-i'] + args: ['-i'] - repo: https://github.com/codespell-project/codespell rev: v2.2.4 hooks: diff --git a/CMakeLists.txt b/CMakeLists.txt index e424747..1beec4e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,16 @@ cmake_minimum_required(VERSION 3.5) project(ros2_control_py) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +add_subdirectory(CppParser) + +if(NOT WIN32) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) @@ -16,15 +26,24 @@ find_package(pybind11 REQUIRED) ament_python_install_package(ros2_control_py) pybind11_add_module(ros2_control_py_core - src/ros2_control_py/core.cpp - src/ros2_control_py/hardware_interface/hardware_interface.cpp - ) + src/ros2_control_py/core.cpp + src/ros2_control_py/hardware_interface/hardware_interface.cpp +) ament_target_dependencies(ros2_control_py_core PUBLIC rclcpp rclcpp_lifecycle - hardware_interface) + hardware_interface +) + +target_include_directories( + ros2_control_py_core + PRIVATE + include +) install(TARGETS ros2_control_py_core - DESTINATION "${PYTHON_INSTALL_DIR}/ros2_control_py") + DESTINATION "${PYTHON_INSTALL_DIR}/ros2_control_py" +) + ament_package() diff --git a/CppParser b/CppParser new file mode 160000 index 0000000..f9a4cfa --- /dev/null +++ b/CppParser @@ -0,0 +1 @@ +Subproject commit f9a4cfac1a3af7286332056d7c661d86b6c35eb3 diff --git a/common b/common new file mode 160000 index 0000000..ddf04d4 --- /dev/null +++ b/common @@ -0,0 +1 @@ +Subproject commit ddf04d4ae1208f364d71b7e8e6ebcbd8d2afe160 diff --git a/src/ros2_control_py/hardware_interface/hardware_interface.hpp b/include/hardware_interface/hardware_interface.hpp similarity index 74% rename from src/ros2_control_py/hardware_interface/hardware_interface.hpp rename to include/hardware_interface/hardware_interface.hpp index 67c2467..3de92d9 100644 --- a/src/ros2_control_py/hardware_interface/hardware_interface.hpp +++ b/include/hardware_interface/hardware_interface.hpp @@ -1,22 +1,22 @@ +// pybind11 #include - -namespace py = pybind11; - -#include +// hardware_interface #include +#include #include +#include #include -#include #include -#include +#include #include -#include +#include #include +#include + +namespace ros2_control_py::bind_hardware_interface { + +namespace py = pybind11; -namespace ros2_control_py -{ -namespace bind_hardware_interface -{ void init_hardware_interface(py::module &m); -} -} + +} // namespace ros2_control_py::bind_hardware_interface diff --git a/src/ros2_control_py/core.cpp b/src/ros2_control_py/core.cpp index bd2c40e..927f4bf 100644 --- a/src/ros2_control_py/core.cpp +++ b/src/ros2_control_py/core.cpp @@ -1,14 +1,15 @@ +// pybind11 #include -#include "hardware_interface/hardware_interface.hpp" +// hardware_interface +#include -PYBIND11_MODULE(core, m) -{ +PYBIND11_MODULE(core, m) { m.doc() = R"( Python bindings for ros2_control functionalities. )"; // Provide custom function signatures - py::options options; + pybind11::options options; options.disable_function_signatures(); // Construct module classes diff --git a/src/ros2_control_py/hardware_interface/hardware_interface.cpp b/src/ros2_control_py/hardware_interface/hardware_interface.cpp index d7dd5d7..1d5e539 100644 --- a/src/ros2_control_py/hardware_interface/hardware_interface.cpp +++ b/src/ros2_control_py/hardware_interface/hardware_interface.cpp @@ -1,193 +1,159 @@ -#include "hardware_interface.hpp" +#include -namespace ros2_control_py -{ -namespace bind_hardware_interface -{ -using namespace hardware_interface; +namespace ros2_control_py::bind_hardware_interface { -void init_hardware_interface(py::module &m) -{ +using namespace hardware_interface; - py::module hardware_interface = - m.def_submodule("hardware_interface"); +void init_hardware_interface(py::module &m) { + py::module hardware_interface = m.def_submodule("hardware_interface"); - py::class_ - (hardware_interface,"InterfaceInfo") + py::class_(hardware_interface, "InterfaceInfo") .def(py::init<>()) - .def_readwrite("name",&InterfaceInfo::name) - .def_readwrite("min",&InterfaceInfo::min) - .def_readwrite("max",&InterfaceInfo::max) - .def_readwrite("initial_value",&InterfaceInfo::initial_value) - .def_readwrite("data_type",&InterfaceInfo::data_type) - .def_readwrite("size",&InterfaceInfo::size); - - - py::class_ - (hardware_interface,"ComponentInfo") + .def_readwrite("name", &InterfaceInfo::name) + .def_readwrite("min", &InterfaceInfo::min) + .def_readwrite("max", &InterfaceInfo::max) + .def_readwrite("initial_value", &InterfaceInfo::initial_value) + .def_readwrite("data_type", &InterfaceInfo::data_type) + .def_readwrite("size", &InterfaceInfo::size); + + py::class_(hardware_interface, "ComponentInfo") .def(py::init<>()) - .def_readwrite("name",&ComponentInfo::name) - .def_readwrite("type",&ComponentInfo::type) - .def_readwrite("command_interfaces", - &ComponentInfo::command_interfaces) - .def_readwrite("state_interfaces", - &ComponentInfo::state_interfaces) - .def_readwrite("parameters", - &ComponentInfo::parameters); + .def_readwrite("name", &ComponentInfo::name) + .def_readwrite("type", &ComponentInfo::type) + .def_readwrite("command_interfaces", &ComponentInfo::command_interfaces) + .def_readwrite("state_interfaces", &ComponentInfo::state_interfaces) + .def_readwrite("parameters", &ComponentInfo::parameters); - py::class_ - (hardware_interface,"ControllerInfo") + py::class_(hardware_interface, "ControllerInfo") .def(py::init<>()) - .def_readwrite("name",&ControllerInfo::name) - .def_readwrite("type",&ControllerInfo::type) - .def_readwrite("claimed_interfaces", - &ControllerInfo::claimed_interfaces); + .def_readwrite("name", &ControllerInfo::name) + .def_readwrite("type", &ControllerInfo::type) + .def_readwrite("claimed_interfaces", &ControllerInfo::claimed_interfaces); - py::class_ - (hardware_interface,"HardwareComponentInfo") + py::class_(hardware_interface, "HardwareComponentInfo") .def(py::init<>()) - .def_readwrite("name",&HardwareComponentInfo::name) - .def_readwrite("type",&HardwareComponentInfo::type) - .def_readwrite("plugin_name",&HardwareComponentInfo::plugin_name) - .def_readwrite("is_async",&HardwareComponentInfo::is_async) - .def_readwrite("state",&HardwareComponentInfo::state) + .def_readwrite("name", &HardwareComponentInfo::name) + .def_readwrite("type", &HardwareComponentInfo::type) + //.def_readwrite("plugin_name", &HardwareComponentInfo::plugin_name) + //.def_readwrite("is_async", &HardwareComponentInfo::is_async) + .def_readwrite("state", &HardwareComponentInfo::state) .def_readwrite("state_interfaces", &HardwareComponentInfo::state_interfaces) .def_readwrite("command_interfaces", - &HardwareComponentInfo::command_interfaces); + &HardwareComponentInfo::command_interfaces); - py::class_ - (hardware_interface,"JointInfo") + py::class_(hardware_interface, "JointInfo") .def(py::init<>()) - .def_readwrite("name",&JointInfo::name) - .def_readwrite("state_interfaces",&JointInfo::state_interfaces) - .def_readwrite("command_interfaces",&JointInfo::command_interfaces) - .def_readwrite("role",&JointInfo::role) - .def_readwrite("mechanical_reduction",&JointInfo::mechanical_reduction) - .def_readwrite("offset",&JointInfo::offset); - - py::class_ - (hardware_interface,"ActuatorInfo") + .def_readwrite("name", &JointInfo::name) + .def_readwrite("state_interfaces", &JointInfo::state_interfaces) + .def_readwrite("command_interfaces", &JointInfo::command_interfaces) + .def_readwrite("role", &JointInfo::role) + .def_readwrite("mechanical_reduction", &JointInfo::mechanical_reduction) + .def_readwrite("offset", &JointInfo::offset); + + py::class_(hardware_interface, "ActuatorInfo") .def(py::init<>()) - .def_readwrite("name",&ActuatorInfo::name) - .def_readwrite("state_interfaces",&ActuatorInfo::state_interfaces) - .def_readwrite("command_interfaces",&ActuatorInfo::command_interfaces) - .def_readwrite("role",&ActuatorInfo::role) - .def_readwrite("mechanical_reduction",&ActuatorInfo::mechanical_reduction) - .def_readwrite("offset",&ActuatorInfo::offset); - - py::class_ - (hardware_interface,"Actuator") + .def_readwrite("name", &ActuatorInfo::name) + .def_readwrite("state_interfaces", &ActuatorInfo::state_interfaces) + .def_readwrite("command_interfaces", &ActuatorInfo::command_interfaces) + .def_readwrite("role", &ActuatorInfo::role) + .def_readwrite("mechanical_reduction", + &ActuatorInfo::mechanical_reduction) + .def_readwrite("offset", &ActuatorInfo::offset); + + py::class_(hardware_interface, "Actuator") .def(py::init<>()) - .def("initialize",&Actuator::initialize) - .def("configure",&Actuator::configure) - .def("cleanup",&Actuator::cleanup) - .def("shutdown",&Actuator::shutdown) - .def("activate",&Actuator::activate) - .def("deactivate",&Actuator::deactivate) - .def("error",&Actuator::error) - .def("export_state_interfaces",&Actuator::export_state_interfaces) - .def("export_command_interfaces",&Actuator::export_command_interfaces) + .def("initialize", &Actuator::initialize) + .def("configure", &Actuator::configure) + .def("cleanup", &Actuator::cleanup) + .def("shutdown", &Actuator::shutdown) + .def("activate", &Actuator::activate) + .def("deactivate", &Actuator::deactivate) + .def("error", &Actuator::error) + .def("export_state_interfaces", &Actuator::export_state_interfaces) + .def("export_command_interfaces", &Actuator::export_command_interfaces) .def("prepare_command_mode_switch", &Actuator::prepare_command_mode_switch) .def("perform_command_mode_switch", &Actuator::perform_command_mode_switch) - .def("get_name",&Actuator::get_name) - .def("get_state",&Actuator::get_state) - .def("read",&Actuator::read) - .def("write",&Actuator::write); + .def("get_name", &Actuator::get_name) + .def("get_state", &Actuator::get_state) + .def("read", &Actuator::read) + .def("write", &Actuator::write); - py::class_ - (hardware_interface,"TransmissionInfo") + py::class_(hardware_interface, "TransmissionInfo") .def(py::init<>()) - .def_readwrite("name",&TransmissionInfo::name) - .def_readwrite("type",&TransmissionInfo::type) - .def_readwrite("joints",&TransmissionInfo::joints) - .def_readwrite("actuators", - &TransmissionInfo::actuators) - .def_readwrite("parameters", - &TransmissionInfo::parameters); - - py::class_ - (hardware_interface,"HardwareInfo") + .def_readwrite("name", &TransmissionInfo::name) + .def_readwrite("type", &TransmissionInfo::type) + .def_readwrite("joints", &TransmissionInfo::joints) + .def_readwrite("actuators", &TransmissionInfo::actuators) + .def_readwrite("parameters", &TransmissionInfo::parameters); + + py::class_(hardware_interface, "HardwareInfo") .def(py::init<>()) - .def_readwrite("name",&HardwareInfo::name) - .def_readwrite("type",&HardwareInfo::type) - .def_readwrite("is_async",&HardwareInfo::is_async) - .def_readwrite("hardware_plugin_name", - &HardwareInfo::hardware_plugin_name) - .def_readwrite("hardware_parameters", - &HardwareInfo::hardware_parameters) - .def_readwrite("joints", - &HardwareInfo::joints) - .def_readwrite("sensors", - &HardwareInfo::sensors) - .def_readwrite("gpios", - &HardwareInfo::gpios) - .def_readwrite("transmissions", - &HardwareInfo::transmissions) - .def_readwrite("original_xml", - &HardwareInfo::original_xml); - - py::class_ - (hardware_interface,"ReadOnlyHandle") + .def_readwrite("name", &HardwareInfo::name) + .def_readwrite("type", &HardwareInfo::type) + //.def_readwrite("is_async", &HardwareInfo::is_async) + //.def_readwrite("hardware_plugin_name", + // &HardwareInfo::hardware_plugin_name) + .def_readwrite("hardware_parameters", &HardwareInfo::hardware_parameters) + .def_readwrite("joints", &HardwareInfo::joints) + .def_readwrite("sensors", &HardwareInfo::sensors) + .def_readwrite("gpios", &HardwareInfo::gpios) + .def_readwrite("transmissions", &HardwareInfo::transmissions) + .def_readwrite("original_xml", &HardwareInfo::original_xml); + + py::class_(hardware_interface, "ReadOnlyHandle") .def(py::init()) - .def("get_name",&ReadOnlyHandle::get_name) - .def("get_interface_name",&ReadOnlyHandle::get_interface_name) - .def("get_prefix_name",&ReadOnlyHandle::get_prefix_name) - .def("get_value",&ReadOnlyHandle::get_value); + .def("get_name", &ReadOnlyHandle::get_name) + .def("get_interface_name", &ReadOnlyHandle::get_interface_name) + .def("get_prefix_name", &ReadOnlyHandle::get_prefix_name) + .def("get_value", &ReadOnlyHandle::get_value); - py::class_ - (hardware_interface,"ReadWriteHandle") + py::class_(hardware_interface, "ReadWriteHandle") .def(py::init()) - .def("set_value",&ReadWriteHandle::get_value); - + .def("set_value", &ReadWriteHandle::get_value); - py::class_ - (hardware_interface,"Sensor") + py::class_(hardware_interface, "Sensor") .def(py::init<>()) - .def("initialize",&Sensor::initialize) - .def("configure",&Sensor::configure) - .def("cleanup",&Sensor::cleanup) - .def("shutdown",&Sensor::shutdown) - .def("activate",&Sensor::activate) - .def("deactivate",&Sensor::deactivate) - .def("error",&Sensor::error) - .def("export_state_interfaces",&Sensor::export_state_interfaces) - .def("get_name",&Sensor::get_name) - .def("get_state",&Sensor::get_state) - .def("read",&Sensor::read); - - py::class_ - (hardware_interface,"System") + .def("initialize", &Sensor::initialize) + .def("configure", &Sensor::configure) + .def("cleanup", &Sensor::cleanup) + .def("shutdown", &Sensor::shutdown) + .def("activate", &Sensor::activate) + .def("deactivate", &Sensor::deactivate) + .def("error", &Sensor::error) + .def("export_state_interfaces", &Sensor::export_state_interfaces) + .def("get_name", &Sensor::get_name) + .def("get_state", &Sensor::get_state) + .def("read", &Sensor::read); + + py::class_(hardware_interface, "System") .def(py::init<>()) - .def("initialize",&System::initialize) - .def("configure",&System::configure) - .def("cleanup",&System::cleanup) - .def("shutdown",&System::shutdown) - .def("activate",&System::activate) - .def("deactivate",&System::deactivate) - .def("error",&System::error) - .def("export_state_interfaces",&System::export_state_interfaces) - .def("export_command_interfaces",&System::export_command_interfaces) - .def("prepare_command_mode_switch", - &System::prepare_command_mode_switch) - .def("perform_command_mode_switch", - &System::perform_command_mode_switch) - .def("get_name",&System::get_name) - .def("get_state",&System::get_state) - .def("read",&System::read) - .def("write",&System::write); - - m.def("parse_control_resources_from_urdf",parse_control_resources_from_urdf); - - py::class_ - (hardware_interface,"HardwareReadWriteStatus") + .def("initialize", &System::initialize) + .def("configure", &System::configure) + .def("cleanup", &System::cleanup) + .def("shutdown", &System::shutdown) + .def("activate", &System::activate) + .def("deactivate", &System::deactivate) + .def("error", &System::error) + .def("export_state_interfaces", &System::export_state_interfaces) + .def("export_command_interfaces", &System::export_command_interfaces) + .def("prepare_command_mode_switch", &System::prepare_command_mode_switch) + .def("perform_command_mode_switch", &System::perform_command_mode_switch) + .def("get_name", &System::get_name) + .def("get_state", &System::get_state) + .def("read", &System::read) + .def("write", &System::write); + + m.def("parse_control_resources_from_urdf", parse_control_resources_from_urdf); + + /*py::class_(hardware_interface, + "HardwareReadWriteStatus") .def(py::init<>()) - .def_readwrite("ok",&HardwareReadWriteStatus::bool) + .def_readwrite("ok", &HardwareReadWriteStatus::bool) .def_readwrite("failed_hardware_names", - &HardwareReadWriteStatus::failed_hardware_names) - -} -} + &HardwareReadWriteStatus::failed_hardware_names);*/ } + +} // namespace ros2_control_py::bind_hardware_interface From baf61e87e2940284d387236b5454518cd44e5e0a Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Thu, 28 Sep 2023 16:39:01 +0200 Subject: [PATCH 02/42] feat: added filegen in makefile from src/ros2_control_py_builder/main.cpp --- CMakeLists.txt | 27 ++++++++++++++++++++------- src/ros2_control_py_builder/main.cpp | 19 +++++++++++++++++++ 2 files changed, 39 insertions(+), 7 deletions(-) create mode 100644 src/ros2_control_py_builder/main.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1beec4e..683097c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,26 +23,39 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) +add_executable(ros2_control_py_builder + src/ros2_control_py_builder/main.cpp +) +target_link_libraries(ros2_control_py_builder + PRIVATE + cppparser +) +add_custom_command( + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface/hardware_interface2.cpp + COMMAND ros2_control_py_builder ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface/hardware_interface2.cpp + DEPENDS ros2_control_py_builder +) + ament_python_install_package(ros2_control_py) pybind11_add_module(ros2_control_py_core src/ros2_control_py/core.cpp src/ros2_control_py/hardware_interface/hardware_interface.cpp + ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface/hardware_interface2.cpp ) - -ament_target_dependencies(ros2_control_py_core PUBLIC +ament_target_dependencies(ros2_control_py_core + PUBLIC rclcpp rclcpp_lifecycle hardware_interface ) - -target_include_directories( - ros2_control_py_core +target_include_directories(ros2_control_py_core PRIVATE include + ${CMAKE_CURRENT_BINARY_DIR} ) - -install(TARGETS ros2_control_py_core +install( + TARGETS ros2_control_py_core DESTINATION "${PYTHON_INSTALL_DIR}/ros2_control_py" ) diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp new file mode 100644 index 0000000..4a56e33 --- /dev/null +++ b/src/ros2_control_py_builder/main.cpp @@ -0,0 +1,19 @@ +#include + +#include +#include +#include + +int main(int argc, char** argv) { + namespace fs = std::filesystem; + if (argc == 2) { + std::cerr << "ros2_control_py_builder: " << argv[1] << std::endl; + fs::path file{argv[1]}; + fs::create_directories(file.parent_path()); + std::ofstream ofs{argv[1], std::ios::out | std::ios::trunc}; + if (!ofs) std::cerr << "error!" << std::endl; + ofs << std::endl; + } + std::cerr << "ros2_control_py_builder has ran!" << std::endl; + return 0; +} From 1497a6075ba7f6dc9246693815db2ef2e65f3e6a Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 29 Sep 2023 12:28:07 +0200 Subject: [PATCH 03/42] feat: parsing hardware_interfaces/ and finding classes --- CMakeLists.txt | 8 +- common | 2 +- src/ros2_control_py/core.cpp | 17 -- .../hardware_interface/hardware_interface.cpp | 159 ------------------ src/ros2_control_py_builder/main.cpp | 103 ++++++++++-- 5 files changed, 97 insertions(+), 192 deletions(-) delete mode 100644 src/ros2_control_py/core.cpp delete mode 100644 src/ros2_control_py/hardware_interface/hardware_interface.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 683097c..63bdab1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -31,17 +31,15 @@ target_link_libraries(ros2_control_py_builder cppparser ) add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface/hardware_interface2.cpp - COMMAND ros2_control_py_builder ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface/hardware_interface2.cpp + OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface_py.cpp + COMMAND ros2_control_py_builder /opt/ros/humble/include/hardware_interface ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface_py.cpp DEPENDS ros2_control_py_builder ) ament_python_install_package(ros2_control_py) pybind11_add_module(ros2_control_py_core - src/ros2_control_py/core.cpp - src/ros2_control_py/hardware_interface/hardware_interface.cpp - ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface/hardware_interface2.cpp + ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface_py.cpp ) ament_target_dependencies(ros2_control_py_core PUBLIC diff --git a/common b/common index ddf04d4..cc977a4 160000 --- a/common +++ b/common @@ -1 +1 @@ -Subproject commit ddf04d4ae1208f364d71b7e8e6ebcbd8d2afe160 +Subproject commit cc977a41f022553f2f222173bdf1583e8bd7ac32 diff --git a/src/ros2_control_py/core.cpp b/src/ros2_control_py/core.cpp deleted file mode 100644 index 927f4bf..0000000 --- a/src/ros2_control_py/core.cpp +++ /dev/null @@ -1,17 +0,0 @@ -// pybind11 -#include -// hardware_interface -#include - -PYBIND11_MODULE(core, m) { - m.doc() = R"( - Python bindings for ros2_control functionalities. - )"; - - // Provide custom function signatures - pybind11::options options; - options.disable_function_signatures(); - - // Construct module classes - ros2_control_py::bind_hardware_interface::init_hardware_interface(m); -} diff --git a/src/ros2_control_py/hardware_interface/hardware_interface.cpp b/src/ros2_control_py/hardware_interface/hardware_interface.cpp deleted file mode 100644 index 1d5e539..0000000 --- a/src/ros2_control_py/hardware_interface/hardware_interface.cpp +++ /dev/null @@ -1,159 +0,0 @@ -#include - -namespace ros2_control_py::bind_hardware_interface { - -using namespace hardware_interface; - -void init_hardware_interface(py::module &m) { - py::module hardware_interface = m.def_submodule("hardware_interface"); - - py::class_(hardware_interface, "InterfaceInfo") - .def(py::init<>()) - .def_readwrite("name", &InterfaceInfo::name) - .def_readwrite("min", &InterfaceInfo::min) - .def_readwrite("max", &InterfaceInfo::max) - .def_readwrite("initial_value", &InterfaceInfo::initial_value) - .def_readwrite("data_type", &InterfaceInfo::data_type) - .def_readwrite("size", &InterfaceInfo::size); - - py::class_(hardware_interface, "ComponentInfo") - .def(py::init<>()) - .def_readwrite("name", &ComponentInfo::name) - .def_readwrite("type", &ComponentInfo::type) - .def_readwrite("command_interfaces", &ComponentInfo::command_interfaces) - .def_readwrite("state_interfaces", &ComponentInfo::state_interfaces) - .def_readwrite("parameters", &ComponentInfo::parameters); - - py::class_(hardware_interface, "ControllerInfo") - .def(py::init<>()) - .def_readwrite("name", &ControllerInfo::name) - .def_readwrite("type", &ControllerInfo::type) - .def_readwrite("claimed_interfaces", &ControllerInfo::claimed_interfaces); - - py::class_(hardware_interface, "HardwareComponentInfo") - .def(py::init<>()) - .def_readwrite("name", &HardwareComponentInfo::name) - .def_readwrite("type", &HardwareComponentInfo::type) - //.def_readwrite("plugin_name", &HardwareComponentInfo::plugin_name) - //.def_readwrite("is_async", &HardwareComponentInfo::is_async) - .def_readwrite("state", &HardwareComponentInfo::state) - .def_readwrite("state_interfaces", - &HardwareComponentInfo::state_interfaces) - .def_readwrite("command_interfaces", - &HardwareComponentInfo::command_interfaces); - - py::class_(hardware_interface, "JointInfo") - .def(py::init<>()) - .def_readwrite("name", &JointInfo::name) - .def_readwrite("state_interfaces", &JointInfo::state_interfaces) - .def_readwrite("command_interfaces", &JointInfo::command_interfaces) - .def_readwrite("role", &JointInfo::role) - .def_readwrite("mechanical_reduction", &JointInfo::mechanical_reduction) - .def_readwrite("offset", &JointInfo::offset); - - py::class_(hardware_interface, "ActuatorInfo") - .def(py::init<>()) - .def_readwrite("name", &ActuatorInfo::name) - .def_readwrite("state_interfaces", &ActuatorInfo::state_interfaces) - .def_readwrite("command_interfaces", &ActuatorInfo::command_interfaces) - .def_readwrite("role", &ActuatorInfo::role) - .def_readwrite("mechanical_reduction", - &ActuatorInfo::mechanical_reduction) - .def_readwrite("offset", &ActuatorInfo::offset); - - py::class_(hardware_interface, "Actuator") - .def(py::init<>()) - .def("initialize", &Actuator::initialize) - .def("configure", &Actuator::configure) - .def("cleanup", &Actuator::cleanup) - .def("shutdown", &Actuator::shutdown) - .def("activate", &Actuator::activate) - .def("deactivate", &Actuator::deactivate) - .def("error", &Actuator::error) - .def("export_state_interfaces", &Actuator::export_state_interfaces) - .def("export_command_interfaces", &Actuator::export_command_interfaces) - .def("prepare_command_mode_switch", - &Actuator::prepare_command_mode_switch) - .def("perform_command_mode_switch", - &Actuator::perform_command_mode_switch) - .def("get_name", &Actuator::get_name) - .def("get_state", &Actuator::get_state) - .def("read", &Actuator::read) - .def("write", &Actuator::write); - - py::class_(hardware_interface, "TransmissionInfo") - .def(py::init<>()) - .def_readwrite("name", &TransmissionInfo::name) - .def_readwrite("type", &TransmissionInfo::type) - .def_readwrite("joints", &TransmissionInfo::joints) - .def_readwrite("actuators", &TransmissionInfo::actuators) - .def_readwrite("parameters", &TransmissionInfo::parameters); - - py::class_(hardware_interface, "HardwareInfo") - .def(py::init<>()) - .def_readwrite("name", &HardwareInfo::name) - .def_readwrite("type", &HardwareInfo::type) - //.def_readwrite("is_async", &HardwareInfo::is_async) - //.def_readwrite("hardware_plugin_name", - // &HardwareInfo::hardware_plugin_name) - .def_readwrite("hardware_parameters", &HardwareInfo::hardware_parameters) - .def_readwrite("joints", &HardwareInfo::joints) - .def_readwrite("sensors", &HardwareInfo::sensors) - .def_readwrite("gpios", &HardwareInfo::gpios) - .def_readwrite("transmissions", &HardwareInfo::transmissions) - .def_readwrite("original_xml", &HardwareInfo::original_xml); - - py::class_(hardware_interface, "ReadOnlyHandle") - .def(py::init()) - .def("get_name", &ReadOnlyHandle::get_name) - .def("get_interface_name", &ReadOnlyHandle::get_interface_name) - .def("get_prefix_name", &ReadOnlyHandle::get_prefix_name) - .def("get_value", &ReadOnlyHandle::get_value); - - py::class_(hardware_interface, "ReadWriteHandle") - .def(py::init()) - .def("set_value", &ReadWriteHandle::get_value); - - py::class_(hardware_interface, "Sensor") - .def(py::init<>()) - .def("initialize", &Sensor::initialize) - .def("configure", &Sensor::configure) - .def("cleanup", &Sensor::cleanup) - .def("shutdown", &Sensor::shutdown) - .def("activate", &Sensor::activate) - .def("deactivate", &Sensor::deactivate) - .def("error", &Sensor::error) - .def("export_state_interfaces", &Sensor::export_state_interfaces) - .def("get_name", &Sensor::get_name) - .def("get_state", &Sensor::get_state) - .def("read", &Sensor::read); - - py::class_(hardware_interface, "System") - .def(py::init<>()) - .def("initialize", &System::initialize) - .def("configure", &System::configure) - .def("cleanup", &System::cleanup) - .def("shutdown", &System::shutdown) - .def("activate", &System::activate) - .def("deactivate", &System::deactivate) - .def("error", &System::error) - .def("export_state_interfaces", &System::export_state_interfaces) - .def("export_command_interfaces", &System::export_command_interfaces) - .def("prepare_command_mode_switch", &System::prepare_command_mode_switch) - .def("perform_command_mode_switch", &System::perform_command_mode_switch) - .def("get_name", &System::get_name) - .def("get_state", &System::get_state) - .def("read", &System::read) - .def("write", &System::write); - - m.def("parse_control_resources_from_urdf", parse_control_resources_from_urdf); - - /*py::class_(hardware_interface, - "HardwareReadWriteStatus") - .def(py::init<>()) - .def_readwrite("ok", &HardwareReadWriteStatus::bool) - .def_readwrite("failed_hardware_names", - &HardwareReadWriteStatus::failed_hardware_names);*/ -} - -} // namespace ros2_control_py::bind_hardware_interface diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index 4a56e33..fbd7ada 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -1,19 +1,102 @@ -#include - +// STL +#include #include #include #include +// CppParser +#include +#include +#include + +#define ASSERT(Assert, ...) \ + do { \ + if (Assert) break; \ + std::cerr << __VA_ARGS__ << std::endl; \ + return 1; \ + } while (false) + +void remove_attributes(std::string& contents) { + auto it = contents.begin(); + while (it != contents.end()) { + it = std::search_n(it, contents.end(), 2, '['); + auto end = std::search_n(it, contents.end(), 2, ']'); + if (end != contents.end()) it = contents.erase(it, end + 2); + } +} + +std::string read_file(const std::string& filename) { + std::string contents; + std::ifstream in(filename, std::ios::in | std::ios::binary); + if (in) { + in.seekg(0, std::ios::end); + size_t size = static_cast(in.tellg()); + contents.resize(size + 3); // For adding last 2 nulls and a new line. + in.seekg(0, std::ios::beg); + in.read(contents.data(), size); + in.close(); + auto len = stripChar(contents.data(), size, '\r'); + assert(len <= size); + remove_attributes(contents); + contents.resize(len + 3); + contents[len] = '\n'; + contents[len + 1] = '\0'; + contents[len + 2] = '\0'; + } + return contents; +} + +CppCompoundPtr parse_file(CppParser& parser, const std::string& filename) { + std::string stm = read_file(filename); + CppCompoundPtr cppCompound = parser.parseStream(stm.data(), stm.size()); + if (!cppCompound) return cppCompound; + cppCompound->name(filename); + return cppCompound; +} int main(int argc, char** argv) { namespace fs = std::filesystem; - if (argc == 2) { - std::cerr << "ros2_control_py_builder: " << argv[1] << std::endl; - fs::path file{argv[1]}; - fs::create_directories(file.parent_path()); - std::ofstream ofs{argv[1], std::ios::out | std::ios::trunc}; - if (!ofs) std::cerr << "error!" << std::endl; - ofs << std::endl; + ASSERT(argc == 3, "Invalid number of command line arguments, expected 2 got " + << argc - 1); + + fs::path src_dir = argv[1]; + fs::path dst = argv[2]; + fs::path dst_dir = fs::path{dst}.parent_path(); + ASSERT(fs::is_directory(src_dir), src_dir << " is not a valid directory"); + ASSERT((fs::is_regular_file(dst) || !fs::exists(dst)) && + dst.extension() == ".cpp", + dst << " is not a valid file"); + + CppParser parser; + parser.addIgnorableMacro("HARDWARE_INTERFACE_PUBLIC"); + + std::vector names; + + for (auto entry : fs::recursive_directory_iterator{src_dir}) { + fs::path const& path = entry.path(); + if (!entry.is_regular_file() || path.extension() != ".hpp" || + path.filename() == "macros.hpp" || + path.filename() == "component_parser.hpp") + continue; + + const CppCompoundPtr ast = parse_file(parser, path.string()); + ASSERT(ast, "Could not parse " << path); + std::cerr << "Parsed " << path << std::endl; + for (const CppObjPtr& obj_ns : ast->members()) { + CppConstCompoundEPtr ns = obj_ns; + if (!ns || !isNamespace(ns) || ns->name() != "hardware_interface") + continue; + for (const CppObjPtr& obj_cls : ns->members()) { + CppConstCompoundEPtr cls = obj_cls; + if (!cls || (!isClass(cls) && !isStruct(cls))) continue; + std::cerr << "Found class: " << cls->name() << std::endl; + } + } } - std::cerr << "ros2_control_py_builder has ran!" << std::endl; + + fs::create_directories(dst_dir); + std::ofstream ofs{dst, std::ios::out | std::ios::trunc}; + if (!ofs) std::cerr << "error!" << std::endl; + ofs << std::endl; + return 0; } From 57ecd1646ef31bc1b70d4c69a511d863dbba6578 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 29 Sep 2023 14:29:31 +0200 Subject: [PATCH 04/42] feat: added basic program structure --- CMakeLists.txt | 17 +-- CppParser | 2 +- common | 2 +- .../hardware_interface/hardware_interface.hpp | 22 ---- src/ros2_control_py_builder/main.cpp | 102 +++++++++++++++--- 5 files changed, 98 insertions(+), 47 deletions(-) delete mode 100644 include/hardware_interface/hardware_interface.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 63bdab1..e0d2a90 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,11 +2,9 @@ cmake_minimum_required(VERSION 3.5) project(ros2_control_py) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) + set(CMAKE_CXX_STANDARD 17) endif() -add_subdirectory(CppParser) - if(NOT WIN32) add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -23,6 +21,10 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) +add_subdirectory(CppParser) + +set(hi_py_cpp ${CMAKE_CURRENT_BINARY_DIR}/src/hardware_interface_py.cpp) + add_executable(ros2_control_py_builder src/ros2_control_py_builder/main.cpp ) @@ -31,15 +33,15 @@ target_link_libraries(ros2_control_py_builder cppparser ) add_custom_command( - OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface_py.cpp - COMMAND ros2_control_py_builder /opt/ros/humble/include/hardware_interface ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface_py.cpp + OUTPUT ${hi_py_cpp} + COMMAND ros2_control_py_builder /opt/ros/humble/include/hardware_interface ${CMAKE_CURRENT_BINARY_DIR} DEPENDS ros2_control_py_builder ) ament_python_install_package(ros2_control_py) pybind11_add_module(ros2_control_py_core - ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface_py.cpp + ${hi_py_cpp} ) ament_target_dependencies(ros2_control_py_core PUBLIC @@ -49,8 +51,7 @@ ament_target_dependencies(ros2_control_py_core ) target_include_directories(ros2_control_py_core PRIVATE - include - ${CMAKE_CURRENT_BINARY_DIR} + ${CMAKE_CURRENT_BINARY_DIR}/include ) install( TARGETS ros2_control_py_core diff --git a/CppParser b/CppParser index f9a4cfa..cbd5634 160000 --- a/CppParser +++ b/CppParser @@ -1 +1 @@ -Subproject commit f9a4cfac1a3af7286332056d7c661d86b6c35eb3 +Subproject commit cbd56344f7287df1455738222f824cff780886e0 diff --git a/common b/common index cc977a4..616f7e9 160000 --- a/common +++ b/common @@ -1 +1 @@ -Subproject commit cc977a41f022553f2f222173bdf1583e8bd7ac32 +Subproject commit 616f7e9c1b10e4c25258c3e2afeea7a2e53bd8a4 diff --git a/include/hardware_interface/hardware_interface.hpp b/include/hardware_interface/hardware_interface.hpp deleted file mode 100644 index 3de92d9..0000000 --- a/include/hardware_interface/hardware_interface.hpp +++ /dev/null @@ -1,22 +0,0 @@ -// pybind11 -#include -// hardware_interface -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ros2_control_py::bind_hardware_interface { - -namespace py = pybind11; - -void init_hardware_interface(py::module &m); - -} // namespace ros2_control_py::bind_hardware_interface diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index fbd7ada..3d93c80 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -12,9 +12,11 @@ do { \ if (Assert) break; \ std::cerr << __VA_ARGS__ << std::endl; \ - return 1; \ + std::exit(1); \ } while (false) +namespace fs = std::filesystem; + void remove_attributes(std::string& contents) { auto it = contents.begin(); while (it != contents.end()) { @@ -53,31 +55,95 @@ CppCompoundPtr parse_file(CppParser& parser, const std::string& filename) { return cppCompound; } +void write_named_hi_py_hpp(const fs::path& inc_hi_dir, + const std::string& name) { + fs::path path = inc_hi_dir / (name + "_py.hpp"); + std::ofstream ofs{path, std::ios::out | std::ios::trunc}; + ASSERT(ofs, "Could not open " << path); + ofs << R"(// pybind11 +#include +// hardware_interface +#include + +namespace ros2_control_py::bind_hardware_interface +{ + +using namespace hardware_interface; +namespace py = pybind11; + +inline void init_)" + << name << R"((py::module &hardware_interface_py) +{ +)"; + // TODO + ofs << R"(} + +} +)"; +} + +void write_hi_py_cpp(const fs::path& hi_py, + const std::vector& names) { + std::ofstream ofs{hi_py, std::ios::out | std::ios::trunc}; + ASSERT(ofs, "could not open " << hi_py); + ofs << R"(// pybind11 +#include +// parse_control_resources_from_urdf +#include + +// hardware_interface_py +)"; + for (const std::string& name : names) + ofs << "#include \n"; + ofs << R"( +namespace py = pybind11; + +PYBIND11_MODULE(hardware_interface_py, m) +{ + m.doc() = R"doc( + Python bindings for ros2_control functionalities. + )doc"; + + // Provide custom function signatures + py::options options; + options.disable_function_signatures(); + + // Construct module classes +)"; + for (const std::string& name : names) + ofs << " ros2_control_py::bind_hardware_interface::init_" << name + << "(m);\n"; + ofs << R"( + m.def("parse_control_resources_from_urdf", hardware_interface::parse_control_resources_from_urdf); +} +)"; +} + int main(int argc, char** argv) { - namespace fs = std::filesystem; ASSERT(argc == 3, "Invalid number of command line arguments, expected 2 got " << argc - 1); - fs::path src_dir = argv[1]; - fs::path dst = argv[2]; - fs::path dst_dir = fs::path{dst}.parent_path(); - ASSERT(fs::is_directory(src_dir), src_dir << " is not a valid directory"); - ASSERT((fs::is_regular_file(dst) || !fs::exists(dst)) && - dst.extension() == ".cpp", - dst << " is not a valid file"); + fs::path hi_dir = argv[1]; + fs::path dst_dir = argv[2]; + ASSERT(fs::is_directory(hi_dir), hi_dir << " is not a valid directory"); + ASSERT(fs::is_directory(dst_dir), dst_dir << " is not a valid file"); CppParser parser; parser.addIgnorableMacro("HARDWARE_INTERFACE_PUBLIC"); std::vector names; - for (auto entry : fs::recursive_directory_iterator{src_dir}) { - fs::path const& path = entry.path(); + for (auto entry : fs::directory_iterator{hi_dir}) { + const fs::path& path = entry.path(); if (!entry.is_regular_file() || path.extension() != ".hpp" || path.filename() == "macros.hpp" || path.filename() == "component_parser.hpp") continue; + std::string filename = path.filename().string(); + names.emplace_back(filename.substr(0, filename.rfind('.'))); + const CppCompoundPtr ast = parse_file(parser, path.string()); ASSERT(ast, "Could not parse " << path); std::cerr << "Parsed " << path << std::endl; @@ -93,10 +159,16 @@ int main(int argc, char** argv) { } } - fs::create_directories(dst_dir); - std::ofstream ofs{dst, std::ios::out | std::ios::trunc}; - if (!ofs) std::cerr << "error!" << std::endl; - ofs << std::endl; + fs::path src_dir = dst_dir / "src"; + fs::path hi_py = src_dir / "hardware_interface_py.cpp"; + fs::path inc_hi_dir = dst_dir / "include" / "hardware_interface"; + + fs::create_directories(src_dir); + fs::create_directories(inc_hi_dir); + + for (const std::string& name : names) write_named_hi_py_hpp(inc_hi_dir, name); + + write_hi_py_cpp(hi_py, names); return 0; } From e5d93235c6ef364abb495f494ee34ac74c793b36 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 29 Sep 2023 16:40:58 +0200 Subject: [PATCH 05/42] feat: finished most of the job, TODO: finish interfaces & inherited ctor --- CMakeLists.txt | 4 +- src/ros2_control_py_builder/main.cpp | 146 +++++++++++++++++++++++++-- 2 files changed, 137 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e0d2a90..95223be 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,11 +2,11 @@ cmake_minimum_required(VERSION 3.5) project(ros2_control_py) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif() if(NOT WIN32) - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-ggdb3 -O0) endif() find_package(ament_cmake REQUIRED) diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index 3d93c80..f1c000d 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -3,10 +3,16 @@ #include #include #include +#include +#include + // CppParser #include #include +#include +#include #include +#include #define ASSERT(Assert, ...) \ do { \ @@ -55,8 +61,81 @@ CppCompoundPtr parse_file(CppParser& parser, const std::string& filename) { return cppCompound; } -void write_named_hi_py_hpp(const fs::path& inc_hi_dir, - const std::string& name) { +template +class Sep; + +template +std::ostream& operator<<(std::ostream& os, const Sep& sep); + +template +class Sep { + public: + Sep(const T& iterable, const U& separator) + : iterable_{iterable}, separator_{separator} {} + + friend std::ostream& operator<< <>(std::ostream&, const Sep&); + + private: + const T& iterable_; + const U& separator_; +}; + +template +std::ostream& operator<<(std::ostream& os, const Sep& sep) { + auto it = std::begin(sep.iterable_); + if (it == std::end(sep.iterable_)) return os; + os << *it; + for (++it; it != std::end(sep.iterable_); ++it) os << sep.separator_ << *it; + return os; +} + +struct Attr { + Attr(const std::string& name) : name{name} {} + + std::string name; +}; + +struct Ctor { + Ctor(std::vector&& args) : args{std::move(args)} {} + + std::vector args; +}; + +struct Memb { + Memb(const std::string& name) : name{name} {} + + std::string name; +}; + +struct Cls { + Cls(const std::string& name, const std::string& mother) + : name{name}, mother{mother} {} + + std::string name; + std::string mother; + std::vector attrs; + std::vector membs; + std::vector ctors; +}; + +std::ostream& operator<<(std::ostream& os, const Cls& cls) { + os << " py::class_<" << cls.name + << (cls.mother.empty() ? "" : ", " + cls.mother) + << ">(hardware_interface_py, \"" << cls.name << "\")"; + for (const Ctor& ctor : cls.ctors) + os << "\n .def(py::init<" << Sep(ctor.args, ", ") << ">())"; + for (const Memb& memb : cls.membs) + os << "\n .def(\"" << memb.name << "\", &" << cls.name + << "::" << memb.name << ")"; + for (const Attr& attr : cls.attrs) + os << "\n .def_readwrite(\"" << attr.name << "\", &" << cls.name + << "::" << attr.name << ")"; + os << ";\n"; + return os; +} + +void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const std::string& name, + const std::vector& classes) { fs::path path = inc_hi_dir / (name + "_py.hpp"); std::ofstream ofs{path, std::ios::out | std::ios::trunc}; ASSERT(ofs, "Could not open " << path); @@ -75,9 +154,8 @@ namespace py = pybind11; inline void init_)" << name << R"((py::module &hardware_interface_py) { -)"; - // TODO - ofs << R"(} +)" << Sep(classes, "\n") + << R"(} } )"; @@ -131,8 +209,10 @@ int main(int argc, char** argv) { CppParser parser; parser.addIgnorableMacro("HARDWARE_INTERFACE_PUBLIC"); + CppWriter writer; - std::vector names; + std::vector headers; + std::unordered_map> classes; for (auto entry : fs::directory_iterator{hi_dir}) { const fs::path& path = entry.path(); @@ -142,11 +222,12 @@ int main(int argc, char** argv) { continue; std::string filename = path.filename().string(); - names.emplace_back(filename.substr(0, filename.rfind('.'))); + headers.emplace_back(filename.substr(0, filename.rfind('.'))); + classes.insert({headers.back(), {}}); const CppCompoundPtr ast = parse_file(parser, path.string()); ASSERT(ast, "Could not parse " << path); - std::cerr << "Parsed " << path << std::endl; + // std::cerr << "Parsed " << path << std::endl; for (const CppObjPtr& obj_ns : ast->members()) { CppConstCompoundEPtr ns = obj_ns; if (!ns || !isNamespace(ns) || ns->name() != "hardware_interface") @@ -154,7 +235,49 @@ int main(int argc, char** argv) { for (const CppObjPtr& obj_cls : ns->members()) { CppConstCompoundEPtr cls = obj_cls; if (!cls || (!isClass(cls) && !isStruct(cls))) continue; - std::cerr << "Found class: " << cls->name() << std::endl; + // std::cerr << "Found class: " << cls->name() << std::endl; + const CppInheritanceListPtr& parents = cls->inheritanceList(); + ASSERT(!parents || parents->size() <= 1, + "Too many parents for " << cls->name()); + bool has_mother = parents && !parents->empty() && + parents->front().inhType == CppAccessType::kPublic; + Cls& cls_rep = classes[headers.back()].emplace_back( + cls->name(), has_mother ? parents->front().baseName : ""); + for (const CppObjPtr& obj_memb : cls->members()) { + if (!isPublic(obj_memb)) continue; + CppConstVarEPtr attr = obj_memb; + if (attr) { + cls_rep.attrs.emplace_back(attr->name()); + } + CppConstructorEPtr ctor = obj_memb; + if (ctor && !ctor->isCopyConstructor() && + !ctor->isMoveConstructor() && !cls->hasPureVirtual()) { + std::vector args; + const CppParamVector* params = ctor->params(); + bool valid = true; + if (params) { + for (const CppObjPtr& param : *params) { + CppConstVarEPtr var = param; + ASSERT(var, "that was not a var"); + std::ostringstream oss; + writer.emitVarType(var->varType(), oss); + if (oss.str() == "Deleter&&" || + oss.str().find("std::unique_ptr<") != std::string::npos) { + valid = false; + break; + } + args.emplace_back(std::move(oss).str()); + } + } + if (valid) cls_rep.ctors.emplace_back(std::move(args)); + } + CppConstFunctionEPtr memb = obj_memb; + if (memb && memb->name_.find("operator") == std::string::npos && + memb->name_ != "get_full_name" && + memb->name_ != "import_component") { + cls_rep.membs.emplace_back(memb->name_); + } + } } } } @@ -166,9 +289,10 @@ int main(int argc, char** argv) { fs::create_directories(src_dir); fs::create_directories(inc_hi_dir); - for (const std::string& name : names) write_named_hi_py_hpp(inc_hi_dir, name); + for (const auto& [name, classes] : classes) + write_named_hi_py_hpp(inc_hi_dir, name, classes); - write_hi_py_cpp(hi_py, names); + write_hi_py_cpp(hi_py, headers); return 0; } From e947a815d1a2028f4525aaf838459e7cb47c6a29 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Mon, 2 Oct 2023 14:45:25 +0200 Subject: [PATCH 06/42] feat: finished interfaces & inherited ctor && added implicit default ctor && organized project in different files --- src/ros2_control_py_builder/main.cpp | 273 +----------------------- src/ros2_control_py_builder/parse.hpp | 154 +++++++++++++ src/ros2_control_py_builder/structs.hpp | 52 +++++ src/ros2_control_py_builder/utils.hpp | 110 ++++++++++ src/ros2_control_py_builder/write.hpp | 146 +++++++++++++ 5 files changed, 471 insertions(+), 264 deletions(-) create mode 100644 src/ros2_control_py_builder/parse.hpp create mode 100644 src/ros2_control_py_builder/structs.hpp create mode 100644 src/ros2_control_py_builder/utils.hpp create mode 100644 src/ros2_control_py_builder/write.hpp diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index f1c000d..4e4e69a 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -1,202 +1,7 @@ -// STL -#include -#include -#include -#include -#include -#include - -// CppParser -#include -#include -#include -#include -#include -#include - -#define ASSERT(Assert, ...) \ - do { \ - if (Assert) break; \ - std::cerr << __VA_ARGS__ << std::endl; \ - std::exit(1); \ - } while (false) - -namespace fs = std::filesystem; - -void remove_attributes(std::string& contents) { - auto it = contents.begin(); - while (it != contents.end()) { - it = std::search_n(it, contents.end(), 2, '['); - auto end = std::search_n(it, contents.end(), 2, ']'); - if (end != contents.end()) it = contents.erase(it, end + 2); - } -} - -std::string read_file(const std::string& filename) { - std::string contents; - std::ifstream in(filename, std::ios::in | std::ios::binary); - if (in) { - in.seekg(0, std::ios::end); - size_t size = static_cast(in.tellg()); - contents.resize(size + 3); // For adding last 2 nulls and a new line. - in.seekg(0, std::ios::beg); - in.read(contents.data(), size); - in.close(); - auto len = stripChar(contents.data(), size, '\r'); - assert(len <= size); - remove_attributes(contents); - contents.resize(len + 3); - contents[len] = '\n'; - contents[len + 1] = '\0'; - contents[len + 2] = '\0'; - } - return contents; -} - -CppCompoundPtr parse_file(CppParser& parser, const std::string& filename) { - std::string stm = read_file(filename); - CppCompoundPtr cppCompound = parser.parseStream(stm.data(), stm.size()); - if (!cppCompound) return cppCompound; - cppCompound->name(filename); - return cppCompound; -} - -template -class Sep; - -template -std::ostream& operator<<(std::ostream& os, const Sep& sep); - -template -class Sep { - public: - Sep(const T& iterable, const U& separator) - : iterable_{iterable}, separator_{separator} {} - - friend std::ostream& operator<< <>(std::ostream&, const Sep&); - - private: - const T& iterable_; - const U& separator_; -}; - -template -std::ostream& operator<<(std::ostream& os, const Sep& sep) { - auto it = std::begin(sep.iterable_); - if (it == std::end(sep.iterable_)) return os; - os << *it; - for (++it; it != std::end(sep.iterable_); ++it) os << sep.separator_ << *it; - return os; -} - -struct Attr { - Attr(const std::string& name) : name{name} {} - - std::string name; -}; - -struct Ctor { - Ctor(std::vector&& args) : args{std::move(args)} {} - - std::vector args; -}; - -struct Memb { - Memb(const std::string& name) : name{name} {} - - std::string name; -}; - -struct Cls { - Cls(const std::string& name, const std::string& mother) - : name{name}, mother{mother} {} - - std::string name; - std::string mother; - std::vector attrs; - std::vector membs; - std::vector ctors; -}; - -std::ostream& operator<<(std::ostream& os, const Cls& cls) { - os << " py::class_<" << cls.name - << (cls.mother.empty() ? "" : ", " + cls.mother) - << ">(hardware_interface_py, \"" << cls.name << "\")"; - for (const Ctor& ctor : cls.ctors) - os << "\n .def(py::init<" << Sep(ctor.args, ", ") << ">())"; - for (const Memb& memb : cls.membs) - os << "\n .def(\"" << memb.name << "\", &" << cls.name - << "::" << memb.name << ")"; - for (const Attr& attr : cls.attrs) - os << "\n .def_readwrite(\"" << attr.name << "\", &" << cls.name - << "::" << attr.name << ")"; - os << ";\n"; - return os; -} - -void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const std::string& name, - const std::vector& classes) { - fs::path path = inc_hi_dir / (name + "_py.hpp"); - std::ofstream ofs{path, std::ios::out | std::ios::trunc}; - ASSERT(ofs, "Could not open " << path); - ofs << R"(// pybind11 -#include -// hardware_interface -#include - -namespace ros2_control_py::bind_hardware_interface -{ - -using namespace hardware_interface; -namespace py = pybind11; - -inline void init_)" - << name << R"((py::module &hardware_interface_py) -{ -)" << Sep(classes, "\n") - << R"(} - -} -)"; -} - -void write_hi_py_cpp(const fs::path& hi_py, - const std::vector& names) { - std::ofstream ofs{hi_py, std::ios::out | std::ios::trunc}; - ASSERT(ofs, "could not open " << hi_py); - ofs << R"(// pybind11 -#include -// parse_control_resources_from_urdf -#include - -// hardware_interface_py -)"; - for (const std::string& name : names) - ofs << "#include \n"; - ofs << R"( -namespace py = pybind11; - -PYBIND11_MODULE(hardware_interface_py, m) -{ - m.doc() = R"doc( - Python bindings for ros2_control functionalities. - )doc"; - - // Provide custom function signatures - py::options options; - options.disable_function_signatures(); - - // Construct module classes -)"; - for (const std::string& name : names) - ofs << " ros2_control_py::bind_hardware_interface::init_" << name - << "(m);\n"; - ofs << R"( - m.def("parse_control_resources_from_urdf", hardware_interface::parse_control_resources_from_urdf); -} -)"; -} +// ros2_control_py_builder +#include "parse.hpp" +#include "utils.hpp" +#include "write.hpp" int main(int argc, char** argv) { ASSERT(argc == 3, "Invalid number of command line arguments, expected 2 got " @@ -207,87 +12,27 @@ int main(int argc, char** argv) { ASSERT(fs::is_directory(hi_dir), hi_dir << " is not a valid directory"); ASSERT(fs::is_directory(dst_dir), dst_dir << " is not a valid file"); - CppParser parser; - parser.addIgnorableMacro("HARDWARE_INTERFACE_PUBLIC"); - CppWriter writer; - std::vector headers; std::unordered_map> classes; - for (auto entry : fs::directory_iterator{hi_dir}) { + for (auto entry : fs::recursive_directory_iterator{hi_dir}) { const fs::path& path = entry.path(); if (!entry.is_regular_file() || path.extension() != ".hpp" || path.filename() == "macros.hpp" || path.filename() == "component_parser.hpp") continue; - std::string filename = path.filename().string(); - headers.emplace_back(filename.substr(0, filename.rfind('.'))); - classes.insert({headers.back(), {}}); - - const CppCompoundPtr ast = parse_file(parser, path.string()); - ASSERT(ast, "Could not parse " << path); - // std::cerr << "Parsed " << path << std::endl; - for (const CppObjPtr& obj_ns : ast->members()) { - CppConstCompoundEPtr ns = obj_ns; - if (!ns || !isNamespace(ns) || ns->name() != "hardware_interface") - continue; - for (const CppObjPtr& obj_cls : ns->members()) { - CppConstCompoundEPtr cls = obj_cls; - if (!cls || (!isClass(cls) && !isStruct(cls))) continue; - // std::cerr << "Found class: " << cls->name() << std::endl; - const CppInheritanceListPtr& parents = cls->inheritanceList(); - ASSERT(!parents || parents->size() <= 1, - "Too many parents for " << cls->name()); - bool has_mother = parents && !parents->empty() && - parents->front().inhType == CppAccessType::kPublic; - Cls& cls_rep = classes[headers.back()].emplace_back( - cls->name(), has_mother ? parents->front().baseName : ""); - for (const CppObjPtr& obj_memb : cls->members()) { - if (!isPublic(obj_memb)) continue; - CppConstVarEPtr attr = obj_memb; - if (attr) { - cls_rep.attrs.emplace_back(attr->name()); - } - CppConstructorEPtr ctor = obj_memb; - if (ctor && !ctor->isCopyConstructor() && - !ctor->isMoveConstructor() && !cls->hasPureVirtual()) { - std::vector args; - const CppParamVector* params = ctor->params(); - bool valid = true; - if (params) { - for (const CppObjPtr& param : *params) { - CppConstVarEPtr var = param; - ASSERT(var, "that was not a var"); - std::ostringstream oss; - writer.emitVarType(var->varType(), oss); - if (oss.str() == "Deleter&&" || - oss.str().find("std::unique_ptr<") != std::string::npos) { - valid = false; - break; - } - args.emplace_back(std::move(oss).str()); - } - } - if (valid) cls_rep.ctors.emplace_back(std::move(args)); - } - CppConstFunctionEPtr memb = obj_memb; - if (memb && memb->name_.find("operator") == std::string::npos && - memb->name_ != "get_full_name" && - memb->name_ != "import_component") { - cls_rep.membs.emplace_back(memb->name_); - } - } - } - } + std::string name = path.lexically_relative(hi_dir).string(); + parse_header(headers, classes, path, name); } fs::path src_dir = dst_dir / "src"; fs::path hi_py = src_dir / "hardware_interface_py.cpp"; fs::path inc_hi_dir = dst_dir / "include" / "hardware_interface"; + fs::path inc_hi_types_dir = inc_hi_dir / "types"; fs::create_directories(src_dir); - fs::create_directories(inc_hi_dir); + fs::create_directories(inc_hi_types_dir); for (const auto& [name, classes] : classes) write_named_hi_py_hpp(inc_hi_dir, name, classes); diff --git a/src/ros2_control_py_builder/parse.hpp b/src/ros2_control_py_builder/parse.hpp new file mode 100644 index 0000000..a73be8c --- /dev/null +++ b/src/ros2_control_py_builder/parse.hpp @@ -0,0 +1,154 @@ +#pragma once + +// STL +#include +#include +// CppParser +#include +#include +#include +#include +#include +#include +#include +// ros2_control_py_builder +#include "structs.hpp" +#include "utils.hpp" + +inline void parse_class_attr(Cls& cls, CppConstVarEPtr attr) { + cls.attrs.emplace_back(attr->name()); +} + +inline void parse_class_ctor(Cls& cls, CppConstructorEPtr ctor, + CppWriter& writer) { + std::vector args; + const CppParamVector* params = ctor->params(); + bool valid = true; + if (params) { + for (const CppObjPtr& param : *params) { + CppConstVarEPtr var = param; + ASSERT(var, "that was not a var"); + std::string arg = str_of_cpp(writer, var->varType()); + if (arg == "Deleter&&" || + arg.find("std::unique_ptr<") != std::string::npos) { + valid = false; + break; + } + args.emplace_back(std::move(arg)); + } + } + if (valid) cls.ctors.emplace_back(std::move(args)); +} + +inline void parse_class_memb(Cls& cls, CppConstFunctionEPtr memb, + CppWriter& writer) { + { + if (isPureVirtual(memb.get())) { + std::vector args; + std::vector args_names; + const CppParamVector* params = memb->params(); + if (params) { + for (const CppObjPtr& param : *params) { + CppConstVarEPtr var = param; + ASSERT(var, "that was not a var"); + std::string arg = str_of_cpp(writer, var); + arg.pop_back(); + arg.pop_back(); + args.emplace_back(std::move(arg)); + args_names.emplace_back(var->name()); + } + } + cls.vmembs.emplace_back(memb->name_, + str_of_cpp(writer, memb->retType_.get()), + cls.name, std::move(args), std::move(args_names)); + } else + cls.membs.emplace_back(memb->name_); + } +} + +inline void parse_class_using( + Cls& cls, std::vector& headers, + std::unordered_map>& classes) { + auto& cls_vec = classes[headers.back()]; + auto it = std::find_if( + cls_vec.begin(), cls_vec.end(), + [cls](const Cls& other) { return other.name == cls.mother; }); + ASSERT(it != cls_vec.end(), "Could not find parent class"); + cls.ctors.insert(cls.ctors.end(), it->ctors.begin(), it->ctors.end()); +} + +inline void parse_class( + std::vector& headers, + std::unordered_map>& classes, + CppWriter& writer, CppConstCompoundEPtr cls) { + const CppInheritanceListPtr& parents = cls->inheritanceList(); + ASSERT(!parents || parents->size() <= 1, + "Too many parents for " << cls->name()); + bool has_mother = parents && !parents->empty() && + parents->front().inhType == CppAccessType::kPublic; + Cls& cls_rep = classes[headers.back()].emplace_back( + cls->name(), has_mother ? parents->front().baseName : ""); + for (const CppObjPtr& obj_memb : cls->members()) { + if (!isPublic(obj_memb)) continue; + CppConstVarEPtr attr = obj_memb; + if (attr) { + parse_class_attr(cls_rep, attr); + continue; + } + CppConstructorEPtr ctor = obj_memb; + if (ctor && !ctor->isCopyConstructor() && !ctor->isMoveConstructor()) { + parse_class_ctor(cls_rep, ctor, writer); + continue; + } + CppConstFunctionEPtr memb = obj_memb; + if (memb && memb->name_.find("operator") == std::string::npos && + memb->name_ != "get_full_name" && memb->name_ != "import_component") { + parse_class_memb(cls_rep, memb, writer); + continue; + } + CppConstUsingDeclEPtr use = obj_memb; + if (use && use->name_ == cls_rep.mother + "::" + cls_rep.mother) { + parse_class_using(cls_rep, headers, classes); + continue; + } + } +} + +inline void parse_namespace( + std::vector& headers, + std::unordered_map>& classes, + CppWriter& writer, CppConstCompoundEPtr ns) { + for (const CppObjPtr& obj_cls : ns->members()) { + CppConstCompoundEPtr compound = obj_cls; + if (!compound) continue; + if (isNamespace(compound)) { + parse_namespace(headers, classes, writer, compound); + continue; + } + if (isClass(compound) || isStruct(compound)) { + parse_class(headers, classes, writer, compound); + continue; + } + } +} + +inline void parse_header( + std::vector& headers, + std::unordered_map>& classes, fs::path path, + const std::string& name) { + CppParser parser; + parser.addIgnorableMacro("HARDWARE_INTERFACE_PUBLIC"); + CppWriter writer; + + headers.emplace_back(name.substr(0, name.rfind('.'))); + classes.insert({headers.back(), {}}); + + const CppCompoundPtr ast = parse_file(parser, path.string()); + ASSERT(ast, "Could not parse " << path); + // std::cerr << "Parsed " << path << std::endl; + for (const CppObjPtr& obj_ns : ast->members()) { + CppConstCompoundEPtr ns = obj_ns; + if (!ns || !isNamespace(ns) || ns->name() != "hardware_interface") continue; + parse_namespace(headers, classes, writer, ns); + } +} diff --git a/src/ros2_control_py_builder/structs.hpp b/src/ros2_control_py_builder/structs.hpp new file mode 100644 index 0000000..4f07a6c --- /dev/null +++ b/src/ros2_control_py_builder/structs.hpp @@ -0,0 +1,52 @@ +#pragma once + +// STL +#include +#include + +struct Attr { + Attr(const std::string& name) : name{name} {} + + std::string name; +}; + +struct Ctor { + Ctor(std::vector&& args) : args{std::move(args)} {} + + std::vector args; +}; + +struct Memb { + Memb(const std::string& name) : name{name} {} + + std::string name; +}; + +struct VMemb { + VMemb(const std::string& name, const std::string& ret_type, + const std::string cls, const std::vector&& args, + const std::vector&& args_names) + : name{name}, + ret_type{ret_type}, + cls{cls}, + args{std::move(args)}, + args_names{std::move(args_names)} {} + + std::string name; + std::string ret_type; + std::string cls; + std::vector args; + std::vector args_names; +}; + +struct Cls { + Cls(const std::string& name, const std::string& mother) + : name{name}, mother{mother} {} + + std::string name; + std::string mother; + std::vector attrs; + std::vector membs; + std::vector ctors; + std::vector vmembs; +}; diff --git a/src/ros2_control_py_builder/utils.hpp b/src/ros2_control_py_builder/utils.hpp new file mode 100644 index 0000000..f824133 --- /dev/null +++ b/src/ros2_control_py_builder/utils.hpp @@ -0,0 +1,110 @@ +#pragma once + +// STL +#include +#include +#include +#include +#include +// CppParser +#include +#include + +/// @brief if not `Assert` print the other args and exit failure +#define ASSERT(Assert, ...) \ + do { \ + if (Assert) break; \ + std::cerr << __VA_ARGS__ << std::endl; \ + std::exit(EXIT_FAILURE); \ + } while (false) + +/// @brief wrapper over a `const T&` and a `const U&`, display all elements of a +/// `T` with a 'U' as a separator +/// @expample std::cout << Sep{std::vector{1, 2, 3}, ", "} << std::endl; // +/// output: `1, 2, 3` +template +class Sep; + +/// @brief removes `[[.*]]` sequences from the string because the parser does +/// not handle them well +inline void remove_attributes(std::string& contents); +/// @brief same as readFile from CppParser but calls remove_attributes +inline std::string read_file(const std::string& filename); +/// @brief same as parseFile from CppParser but calls read_file instead of +/// readFile +inline CppCompoundPtr parse_file(CppParser& parser, + const std::string& filename); +/// @brief ostream writer for `Sep` +template +std::ostream& operator<<(std::ostream& os, const Sep& sep); +/// @brief wrapper to get a string repr of a CppObj +inline std::string str_of_cpp(CppWriter& writer, const CppObj* cppObj); + +// Impl + +inline void remove_attributes(std::string& contents) { + auto it = contents.begin(); + while (it != contents.end()) { + it = std::search_n(it, contents.end(), 2, '['); + auto end = std::search_n(it, contents.end(), 2, ']'); + if (end != contents.end()) it = contents.erase(it, end + 2); + } +} + +inline std::string read_file(const std::string& filename) { + std::string contents; + std::ifstream in(filename, std::ios::in | std::ios::binary); + if (in) { + in.seekg(0, std::ios::end); + size_t size = static_cast(in.tellg()); + contents.resize(size + 3); // For adding last 2 nulls and a new line. + in.seekg(0, std::ios::beg); + in.read(contents.data(), size); + in.close(); + auto len = stripChar(contents.data(), size, '\r'); + assert(len <= size); + remove_attributes(contents); + contents.resize(len + 3); + contents[len] = '\n'; + contents[len + 1] = '\0'; + contents[len + 2] = '\0'; + } + return contents; +} + +inline CppCompoundPtr parse_file(CppParser& parser, + const std::string& filename) { + std::string stm = read_file(filename); + CppCompoundPtr cppCompound = parser.parseStream(stm.data(), stm.size()); + if (!cppCompound) return cppCompound; + cppCompound->name(filename); + return cppCompound; +} + +template +class Sep { + public: + Sep(const T& iterable, const U& separator) + : iterable_{iterable}, separator_{separator} {} + + friend std::ostream& operator<< <>(std::ostream&, const Sep&); + + private: + const T& iterable_; + const U& separator_; +}; + +template +std::ostream& operator<<(std::ostream& os, const Sep& sep) { + auto it = std::begin(sep.iterable_); + if (it == std::end(sep.iterable_)) return os; + os << *it; + for (++it; it != std::end(sep.iterable_); ++it) os << sep.separator_ << *it; + return os; +} + +inline std::string str_of_cpp(CppWriter& writer, const CppObj* cppObj) { + std::ostringstream oss; + writer.emit(cppObj, oss); + return std::move(oss).str(); +} diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp new file mode 100644 index 0000000..a010a2a --- /dev/null +++ b/src/ros2_control_py_builder/write.hpp @@ -0,0 +1,146 @@ +#pragma once + +// STL +#include +#include +// ros2_control_pŷ_builder +#include "structs.hpp" +#include "utils.hpp" + +namespace fs = std::filesystem; + +/// @brief output a `Cls` to write an hpp +inline std::ostream& operator<<(std::ostream& os, const Cls& cls); +/// @brief output a `VMemb` to write an hpp +inline std::ostream& operator<<(std::ostream& os, const VMemb& vmemb); +/// @brief write a hpp file in `inc_hi_dir` for header `name` with classes +/// `classes` +inline void write_named_hi_py_hpp(const fs::path& inc_hi_dir, + const std::string& name, + const std::vector& classes); +/// @brief write a cpp file in `hi_py` calling bindings from headers `names` +inline void write_hi_py_cpp(const fs::path& hi_py, + const std::vector& names); + +// Impl + +inline std::ostream& operator<<(std::ostream& os, const Cls& cls) { + if (!cls.vmembs.empty()) { + Cls py_cls{"Py" + cls.name, cls.name}; + py_cls.attrs = cls.attrs; + py_cls.ctors = cls.ctors; + py_cls.membs = cls.membs; + py_cls.membs.reserve(py_cls.membs.size() + cls.vmembs.size()); + for (const VMemb& vmemb : cls.vmembs) py_cls.membs.emplace_back(vmemb.name); + return os << py_cls; + } + os << " py::class_<" << cls.name + << (cls.mother.empty() ? "" : ", " + cls.mother) + << ">(hardware_interface_py, \"" << cls.name << "\")"; + std::vector ctors = cls.ctors; + if (ctors.empty()) ctors.emplace_back(std::vector{}); + for (const Ctor& ctor : ctors) + os << "\n .def(py::init<" << Sep(ctor.args, ", ") << ">())"; + for (const Memb& memb : cls.membs) + os << "\n .def(\"" << memb.name << "\", &" << cls.name + << "::" << memb.name << ")"; + for (const Attr& attr : cls.attrs) + os << "\n .def_readwrite(\"" << attr.name << "\", &" << cls.name + << "::" << attr.name << ")"; + os << ";\n"; + return os; +} + +inline std::ostream& operator<<(std::ostream& os, const VMemb& vmemb) { + os << "\n " << vmemb.ret_type << ' ' << vmemb.name << '(' + << Sep(vmemb.args, ", ") << R"() override { + PYBIND11_OVERRIDE_PURE( + )" + << vmemb.ret_type << R"(, + )" + << vmemb.cls << R"(, + )" + << vmemb.name; + for (const std::string& arg : vmemb.args_names) os << ",\n " << arg; + return os << R"( + ); + } +)"; +} + +void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const std::string& name, + const std::vector& classes) { + fs::path path = inc_hi_dir / (name + "_py.hpp"); + std::ofstream ofs{path, std::ios::out | std::ios::trunc}; + ASSERT(ofs, "Could not open " << path); + ofs << R"(// pybind11 +#include +// hardware_interface +#include + +namespace ros2_control_py::bind_hardware_interface +{ + +using namespace hardware_interface; +namespace py = pybind11; +)"; + for (const Cls& cls : classes) { + if (cls.vmembs.empty()) continue; + ofs << "\nclass Py" << cls.name << ": public " << cls.name + << "{\n public:\n using " << cls.name << "::" << cls.name << ";\n"; + for (const VMemb& vmemb : cls.vmembs) ofs << vmemb; + ofs << "};\n"; + } + std::string proper = name; + std::replace(proper.begin(), proper.end(), '/', '_'); + ofs << R"( +inline void init_)" + << proper << R"((py::module &hardware_interface_py) +{ +)" << Sep(classes, "\n") + << R"(} + +} +)"; +} + +void write_hi_py_cpp(const fs::path& hi_py, + const std::vector& names) { + std::ofstream ofs{hi_py, std::ios::out | std::ios::trunc}; + ASSERT(ofs, "could not open " << hi_py); + ofs << R"(// pybind11 +#include +// parse_control_resources_from_urdf +#include + +// hardware_interface_py +)"; + for (const std::string& name : names) + ofs << "#include \n"; + ofs << R"( +namespace py = pybind11; + +PYBIND11_MODULE(hardware_interface_py, m) +{ + m.doc() = R"doc( + Python bindings for ros2_control functionalities. + )doc"; + + // Provide custom function signatures + py::options options; + options.disable_function_signatures(); + + // Construct module classes +)"; + for (const std::string& name : names) { + std::string proper = name; + std::replace(proper.begin(), proper.end(), '/', '_'); + ofs << " ros2_control_py::bind_hardware_interface::init_" << proper + << "(m);\n"; + } + ofs << R"( + m.def("parse_control_resources_from_urdf", hardware_interface::parse_control_resources_from_urdf); +} +)"; +} From 58de694626f6827ea2de81cf3dbda645a967d3a7 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Mon, 2 Oct 2023 15:39:36 +0200 Subject: [PATCH 07/42] feat: finished builder, added enums and string-ish vars --- src/ros2_control_py_builder/main.cpp | 9 ++-- src/ros2_control_py_builder/parse.hpp | 66 ++++++++++++++----------- src/ros2_control_py_builder/structs.hpp | 19 +++++++ src/ros2_control_py_builder/write.hpp | 57 +++++++++++++-------- 4 files changed, 96 insertions(+), 55 deletions(-) diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index 4e4e69a..ace9308 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -12,8 +12,7 @@ int main(int argc, char** argv) { ASSERT(fs::is_directory(hi_dir), hi_dir << " is not a valid directory"); ASSERT(fs::is_directory(dst_dir), dst_dir << " is not a valid file"); - std::vector headers; - std::unordered_map> classes; + std::vector
headers; for (auto entry : fs::recursive_directory_iterator{hi_dir}) { const fs::path& path = entry.path(); @@ -23,7 +22,7 @@ int main(int argc, char** argv) { continue; std::string name = path.lexically_relative(hi_dir).string(); - parse_header(headers, classes, path, name); + parse_header(headers, path, name); } fs::path src_dir = dst_dir / "src"; @@ -34,8 +33,8 @@ int main(int argc, char** argv) { fs::create_directories(src_dir); fs::create_directories(inc_hi_types_dir); - for (const auto& [name, classes] : classes) - write_named_hi_py_hpp(inc_hi_dir, name, classes); + for (const Header& header : headers) + write_named_hi_py_hpp(inc_hi_dir, header); write_hi_py_cpp(hi_py, headers); diff --git a/src/ros2_control_py_builder/parse.hpp b/src/ros2_control_py_builder/parse.hpp index a73be8c..b4a6329 100644 --- a/src/ros2_control_py_builder/parse.hpp +++ b/src/ros2_control_py_builder/parse.hpp @@ -2,7 +2,6 @@ // STL #include -#include // CppParser #include #include @@ -66,27 +65,22 @@ inline void parse_class_memb(Cls& cls, CppConstFunctionEPtr memb, } } -inline void parse_class_using( - Cls& cls, std::vector& headers, - std::unordered_map>& classes) { - auto& cls_vec = classes[headers.back()]; +inline void parse_class_using(Cls& cls, Header& header) { auto it = std::find_if( - cls_vec.begin(), cls_vec.end(), + header.classes.begin(), header.classes.end(), [cls](const Cls& other) { return other.name == cls.mother; }); - ASSERT(it != cls_vec.end(), "Could not find parent class"); + ASSERT(it != header.classes.end(), "Could not find parent class"); cls.ctors.insert(cls.ctors.end(), it->ctors.begin(), it->ctors.end()); } -inline void parse_class( - std::vector& headers, - std::unordered_map>& classes, - CppWriter& writer, CppConstCompoundEPtr cls) { +inline void parse_class(Header& header, CppWriter& writer, + CppConstCompoundEPtr cls) { const CppInheritanceListPtr& parents = cls->inheritanceList(); ASSERT(!parents || parents->size() <= 1, "Too many parents for " << cls->name()); bool has_mother = parents && !parents->empty() && parents->front().inhType == CppAccessType::kPublic; - Cls& cls_rep = classes[headers.back()].emplace_back( + Cls& cls_rep = header.classes.emplace_back( cls->name(), has_mother ? parents->front().baseName : ""); for (const CppObjPtr& obj_memb : cls->members()) { if (!isPublic(obj_memb)) continue; @@ -108,40 +102,52 @@ inline void parse_class( } CppConstUsingDeclEPtr use = obj_memb; if (use && use->name_ == cls_rep.mother + "::" + cls_rep.mother) { - parse_class_using(cls_rep, headers, classes); + parse_class_using(cls_rep, header); continue; } } } -inline void parse_namespace( - std::vector& headers, - std::unordered_map>& classes, - CppWriter& writer, CppConstCompoundEPtr ns) { - for (const CppObjPtr& obj_cls : ns->members()) { - CppConstCompoundEPtr compound = obj_cls; - if (!compound) continue; - if (isNamespace(compound)) { - parse_namespace(headers, classes, writer, compound); +inline void parse_enum(Header& header, CppEnumEPtr enu) { + header.enums.emplace_back(enu->name_); + for (const auto& item : *enu->itemList_) + header.enums.back().items.emplace_back(item->name_); +} + +inline void parse_var(Header& header, CppVarEPtr var) { + header.vars.emplace_back(var->name()); +} + +inline void parse_namespace(Header& header, CppWriter& writer, + CppConstCompoundEPtr ns, std::string name) { + header.namespaces.emplace_back(name); + for (const CppObjPtr& obj : ns->members()) { + CppVarEPtr var = obj; + if (var) { + parse_var(header, var); continue; } - if (isClass(compound) || isStruct(compound)) { - parse_class(headers, classes, writer, compound); + CppEnumEPtr enu = obj; + if (enu && enu->itemList_) { + parse_enum(header, enu); continue; } + CppConstCompoundEPtr compound = obj; + if (!compound) continue; + if (isNamespace(compound)) + parse_namespace(header, writer, compound, name + "::" + compound->name()); + else if (isClass(compound) || isStruct(compound)) + parse_class(header, writer, compound); } } -inline void parse_header( - std::vector& headers, - std::unordered_map>& classes, fs::path path, - const std::string& name) { +inline void parse_header(std::vector
& headers, fs::path path, + const std::string& name) { CppParser parser; parser.addIgnorableMacro("HARDWARE_INTERFACE_PUBLIC"); CppWriter writer; headers.emplace_back(name.substr(0, name.rfind('.'))); - classes.insert({headers.back(), {}}); const CppCompoundPtr ast = parse_file(parser, path.string()); ASSERT(ast, "Could not parse " << path); @@ -149,6 +155,6 @@ inline void parse_header( for (const CppObjPtr& obj_ns : ast->members()) { CppConstCompoundEPtr ns = obj_ns; if (!ns || !isNamespace(ns) || ns->name() != "hardware_interface") continue; - parse_namespace(headers, classes, writer, ns); + parse_namespace(headers.back(), writer, ns, ns->name()); } } diff --git a/src/ros2_control_py_builder/structs.hpp b/src/ros2_control_py_builder/structs.hpp index 4f07a6c..5aa59a6 100644 --- a/src/ros2_control_py_builder/structs.hpp +++ b/src/ros2_control_py_builder/structs.hpp @@ -50,3 +50,22 @@ struct Cls { std::vector ctors; std::vector vmembs; }; + +struct Enum { + std::string name; + std::vector items; +}; + +struct Var { + std::string name; +}; + +struct Header { + Header(const std::string& name) : name{name} {} + + std::string name; + std::vector namespaces; + std::vector classes; + std::vector enums; + std::vector vars; +}; diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index a010a2a..a1d60b6 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -13,14 +13,16 @@ namespace fs = std::filesystem; inline std::ostream& operator<<(std::ostream& os, const Cls& cls); /// @brief output a `VMemb` to write an hpp inline std::ostream& operator<<(std::ostream& os, const VMemb& vmemb); -/// @brief write a hpp file in `inc_hi_dir` for header `name` with classes -/// `classes` +/// @brief output a `Enum` to write an hpp +inline std::ostream& operator<<(std::ostream& os, const Enum& enu); +/// @brief output a `Var` to write an hpp +inline std::ostream& operator<<(std::ostream& os, const Var& var); +/// @brief write a hpp file in `inc_hi_dir` for header `header` inline void write_named_hi_py_hpp(const fs::path& inc_hi_dir, - const std::string& name, - const std::vector& classes); -/// @brief write a cpp file in `hi_py` calling bindings from headers `names` + const Header& header); +/// @brief write a cpp file in `hi_py` calling bindings from headers `headers` inline void write_hi_py_cpp(const fs::path& hi_py, - const std::vector& names); + const std::vector
& headers); // Impl @@ -68,45 +70,60 @@ inline std::ostream& operator<<(std::ostream& os, const VMemb& vmemb) { )"; } -void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const std::string& name, - const std::vector& classes) { - fs::path path = inc_hi_dir / (name + "_py.hpp"); +inline std::ostream& operator<<(std::ostream& os, const Enum& enu) { + ASSERT(!enu.items.empty(), "empty enum"); + os << " py::enum_<" << enu.name << ">(hardware_interface_py, \"" << enu.name + << "\")\n"; + for (const std::string& item : enu.items) + os << " .value(\"" << item << "\", " << enu.name << "::" << item + << ")\n"; + return os << " .export_values();\n"; +} + +inline std::ostream& operator<<(std::ostream& os, const Var& var) { + return os << " hardware_interface_py.def(\"" << var.name + << "\", []() { return std::string{" << var.name << "}; });"; +} + +void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const Header& header) { + fs::path path = inc_hi_dir / (header.name + "_py.hpp"); std::ofstream ofs{path, std::ios::out | std::ios::trunc}; ASSERT(ofs, "Could not open " << path); ofs << R"(// pybind11 #include // hardware_interface #include + << header.name << R"(.hpp> namespace ros2_control_py::bind_hardware_interface { -using namespace hardware_interface; namespace py = pybind11; )"; - for (const Cls& cls : classes) { + for (const std::string& ns : header.namespaces) + ofs << "using namespace " << ns << ";\n"; + for (const Cls& cls : header.classes) { if (cls.vmembs.empty()) continue; ofs << "\nclass Py" << cls.name << ": public " << cls.name << "{\n public:\n using " << cls.name << "::" << cls.name << ";\n"; for (const VMemb& vmemb : cls.vmembs) ofs << vmemb; ofs << "};\n"; } - std::string proper = name; + std::string proper = header.name; std::replace(proper.begin(), proper.end(), '/', '_'); ofs << R"( inline void init_)" << proper << R"((py::module &hardware_interface_py) { -)" << Sep(classes, "\n") - << R"(} +)" << Sep(header.vars, "\n") + << Sep(header.enums, "\n") << Sep(header.classes, "\n") << R"(} } )"; } void write_hi_py_cpp(const fs::path& hi_py, - const std::vector& names) { + const std::vector
& headers) { std::ofstream ofs{hi_py, std::ios::out | std::ios::trunc}; ASSERT(ofs, "could not open " << hi_py); ofs << R"(// pybind11 @@ -116,8 +133,8 @@ void write_hi_py_cpp(const fs::path& hi_py, // hardware_interface_py )"; - for (const std::string& name : names) - ofs << "#include \n"; + for (const Header& header : headers) + ofs << "#include \n"; ofs << R"( namespace py = pybind11; @@ -133,8 +150,8 @@ PYBIND11_MODULE(hardware_interface_py, m) // Construct module classes )"; - for (const std::string& name : names) { - std::string proper = name; + for (const Header& header : headers) { + std::string proper = header.name; std::replace(proper.begin(), proper.end(), '/', '_'); ofs << " ros2_control_py::bind_hardware_interface::init_" << proper << "(m);\n"; From dbb1f58990d9e270068d020fe645cb59399151cb Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Mon, 2 Oct 2023 15:43:26 +0200 Subject: [PATCH 08/42] Create humble.yml --- .github/workflows/humble.yml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .github/workflows/humble.yml diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml new file mode 100644 index 0000000..5db9ead --- /dev/null +++ b/.github/workflows/humble.yml @@ -0,0 +1,21 @@ +name: humble + +on: + push: + branches: [ "main" ] + pull_request: + branches: [ "main" ] + +jobs: + build-humble: + runs-on: ubuntu-latest + container: ubuntu:jammy + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: humble + - uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: ros2_control_py + target-ros2-distro: humble From 70bee341c30c24ea32194478649ca0df3076f15a Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Mon, 2 Oct 2023 15:44:51 +0200 Subject: [PATCH 09/42] feat(CI): added rolling.yml --- .github/workflows/rolling.yml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .github/workflows/rolling.yml diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml new file mode 100644 index 0000000..510c5df --- /dev/null +++ b/.github/workflows/rolling.yml @@ -0,0 +1,21 @@ +name: rolling + +on: + push: + branches: [ "main" ] + pull_request: + branches: [ "main" ] + +jobs: + build-humble: + runs-on: ubuntu-latest + container: ubuntu:jammy + steps: + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@v0.7 + with: + required-ros-distributions: rolling + - uses: ros-tooling/action-ros-ci@v0.3 + with: + package-name: ros2_control_py + target-ros2-distro: rolling From 161b783862345057fd9d17464bdf4cd52756126f Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Mon, 2 Oct 2023 15:53:06 +0200 Subject: [PATCH 10/42] fix: removed checkout before action-ros-ci && removed typo --- .github/workflows/humble.yml | 1 - .github/workflows/rolling.yml | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/.github/workflows/humble.yml b/.github/workflows/humble.yml index 5db9ead..2dca0c9 100644 --- a/.github/workflows/humble.yml +++ b/.github/workflows/humble.yml @@ -11,7 +11,6 @@ jobs: runs-on: ubuntu-latest container: ubuntu:jammy steps: - - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: humble diff --git a/.github/workflows/rolling.yml b/.github/workflows/rolling.yml index 510c5df..6dfd1b1 100644 --- a/.github/workflows/rolling.yml +++ b/.github/workflows/rolling.yml @@ -7,11 +7,10 @@ on: branches: [ "main" ] jobs: - build-humble: + build-rolling: runs-on: ubuntu-latest container: ubuntu:jammy steps: - - uses: actions/checkout@v3 - uses: ros-tooling/setup-ros@v0.7 with: required-ros-distributions: rolling From 24266243bf7b400223db512170eb773593de251a Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Mon, 2 Oct 2023 16:48:10 +0200 Subject: [PATCH 11/42] feat: removed custom modification and pushed them to a separate remote at https://github.com/VokunGahrotLaas/fork-cppparser --- .gitmodules | 9 +++------ CMakeLists.txt | 2 +- CppParser | 1 - common | 1 - cppparser | 1 + src/ros2_control_py_builder/main.cpp | 24 ++++++++++++------------ src/ros2_control_py_builder/parse.hpp | 5 ++++- src/ros2_control_py_builder/write.hpp | 12 ++++++------ 8 files changed, 27 insertions(+), 28 deletions(-) delete mode 160000 CppParser delete mode 160000 common create mode 160000 cppparser diff --git a/.gitmodules b/.gitmodules index 5b69abf..bd009d9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ -[submodule "common"] - path = common - url = https://github.com/satya-das/common.git -[submodule "CppParser"] - path = CppParser - url = https://github.com/satya-das/CppParser.git +[submodule "cppparser"] + path = cppparser + url = https://github.com/VokunGahrotLaas/fork-cppparser diff --git a/CMakeLists.txt b/CMakeLists.txt index 95223be..1035154 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) -add_subdirectory(CppParser) +add_subdirectory(cppparser) set(hi_py_cpp ${CMAKE_CURRENT_BINARY_DIR}/src/hardware_interface_py.cpp) diff --git a/CppParser b/CppParser deleted file mode 160000 index cbd5634..0000000 --- a/CppParser +++ /dev/null @@ -1 +0,0 @@ -Subproject commit cbd56344f7287df1455738222f824cff780886e0 diff --git a/common b/common deleted file mode 160000 index 616f7e9..0000000 --- a/common +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 616f7e9c1b10e4c25258c3e2afeea7a2e53bd8a4 diff --git a/cppparser b/cppparser new file mode 160000 index 0000000..1ffb755 --- /dev/null +++ b/cppparser @@ -0,0 +1 @@ +Subproject commit 1ffb7550b29f38ded92679bb92c2982875595ae4 diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index ace9308..b4cd928 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -7,15 +7,15 @@ int main(int argc, char** argv) { ASSERT(argc == 3, "Invalid number of command line arguments, expected 2 got " << argc - 1); - fs::path hi_dir = argv[1]; - fs::path dst_dir = argv[2]; - ASSERT(fs::is_directory(hi_dir), hi_dir << " is not a valid directory"); - ASSERT(fs::is_directory(dst_dir), dst_dir << " is not a valid file"); + sfs::path hi_dir = argv[1]; + sfs::path dst_dir = argv[2]; + ASSERT(sfs::is_directory(hi_dir), hi_dir << " is not a valid directory"); + ASSERT(sfs::is_directory(dst_dir), dst_dir << " is not a valid file"); std::vector
headers; - for (auto entry : fs::recursive_directory_iterator{hi_dir}) { - const fs::path& path = entry.path(); + for (auto entry : sfs::recursive_directory_iterator{hi_dir}) { + const sfs::path& path = entry.path(); if (!entry.is_regular_file() || path.extension() != ".hpp" || path.filename() == "macros.hpp" || path.filename() == "component_parser.hpp") @@ -25,13 +25,13 @@ int main(int argc, char** argv) { parse_header(headers, path, name); } - fs::path src_dir = dst_dir / "src"; - fs::path hi_py = src_dir / "hardware_interface_py.cpp"; - fs::path inc_hi_dir = dst_dir / "include" / "hardware_interface"; - fs::path inc_hi_types_dir = inc_hi_dir / "types"; + sfs::path src_dir = dst_dir / "src"; + sfs::path hi_py = src_dir / "hardware_interface_py.cpp"; + sfs::path inc_hi_dir = dst_dir / "include" / "hardware_interface"; + sfs::path inc_hi_types_dir = inc_hi_dir / "types"; - fs::create_directories(src_dir); - fs::create_directories(inc_hi_types_dir); + sfs::create_directories(src_dir); + sfs::create_directories(inc_hi_types_dir); for (const Header& header : headers) write_named_hi_py_hpp(inc_hi_dir, header); diff --git a/src/ros2_control_py_builder/parse.hpp b/src/ros2_control_py_builder/parse.hpp index b4a6329..bce840d 100644 --- a/src/ros2_control_py_builder/parse.hpp +++ b/src/ros2_control_py_builder/parse.hpp @@ -2,6 +2,7 @@ // STL #include +#include // CppParser #include #include @@ -14,6 +15,8 @@ #include "structs.hpp" #include "utils.hpp" +namespace sfs = std::filesystem; + inline void parse_class_attr(Cls& cls, CppConstVarEPtr attr) { cls.attrs.emplace_back(attr->name()); } @@ -141,7 +144,7 @@ inline void parse_namespace(Header& header, CppWriter& writer, } } -inline void parse_header(std::vector
& headers, fs::path path, +inline void parse_header(std::vector
& headers, sfs::path path, const std::string& name) { CppParser parser; parser.addIgnorableMacro("HARDWARE_INTERFACE_PUBLIC"); diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index a1d60b6..70c6183 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -7,7 +7,7 @@ #include "structs.hpp" #include "utils.hpp" -namespace fs = std::filesystem; +namespace sfs = std::filesystem; /// @brief output a `Cls` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Cls& cls); @@ -18,10 +18,10 @@ inline std::ostream& operator<<(std::ostream& os, const Enum& enu); /// @brief output a `Var` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Var& var); /// @brief write a hpp file in `inc_hi_dir` for header `header` -inline void write_named_hi_py_hpp(const fs::path& inc_hi_dir, +inline void write_named_hi_py_hpp(const sfs::path& inc_hi_dir, const Header& header); /// @brief write a cpp file in `hi_py` calling bindings from headers `headers` -inline void write_hi_py_cpp(const fs::path& hi_py, +inline void write_hi_py_cpp(const sfs::path& hi_py, const std::vector
& headers); // Impl @@ -85,8 +85,8 @@ inline std::ostream& operator<<(std::ostream& os, const Var& var) { << "\", []() { return std::string{" << var.name << "}; });"; } -void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const Header& header) { - fs::path path = inc_hi_dir / (header.name + "_py.hpp"); +void write_named_hi_py_hpp(const sfs::path& inc_hi_dir, const Header& header) { + sfs::path path = inc_hi_dir / (header.name + "_py.hpp"); std::ofstream ofs{path, std::ios::out | std::ios::trunc}; ASSERT(ofs, "Could not open " << path); ofs << R"(// pybind11 @@ -122,7 +122,7 @@ inline void init_)" )"; } -void write_hi_py_cpp(const fs::path& hi_py, +void write_hi_py_cpp(const sfs::path& hi_py, const std::vector
& headers) { std::ofstream ofs{hi_py, std::ios::out | std::ios::trunc}; ASSERT(ofs, "could not open " << hi_py); From 20710ca6a04b1204398e87018963fcfed2e88230 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 4 Oct 2023 09:03:47 +0200 Subject: [PATCH 12/42] fix: added flex to rosdeps --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index e6fddcd..053e373 100644 --- a/package.xml +++ b/package.xml @@ -17,6 +17,8 @@ ament_index_python + flex + rclcpp rclcpp_lifecycle rclpy From 9df5babb626f24099a39da815ab9da10a9b100d0 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 4 Oct 2023 10:55:50 +0200 Subject: [PATCH 13/42] feat: finished making the project work --- CMakeLists.txt | 27 +++++++++-------------- README.md | 29 +++++++++++++++++++++++++ ros2_control_py/__init__.py | 2 +- src/ros2_control_py_builder/main.cpp | 26 +++++++++++----------- src/ros2_control_py_builder/parse.hpp | 7 +++--- src/ros2_control_py_builder/structs.hpp | 4 ++++ src/ros2_control_py_builder/write.hpp | 21 +++++++++--------- 7 files changed, 72 insertions(+), 44 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1035154..ca1a4e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,20 +2,17 @@ cmake_minimum_required(VERSION 3.5) project(ros2_control_py) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 20) -endif() - -if(NOT WIN32) - add_compile_options(-ggdb3 -O0) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_EXTENSIONS OFF) endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(rclpy REQUIRED) -find_package(controller_interface REQUIRED) -find_package(controller_manager REQUIRED) -find_package(controller_manager_msgs REQUIRED) +#find_package(controller_interface REQUIRED) +#find_package(controller_manager REQUIRED) +#find_package(controller_manager_msgs REQUIRED) find_package(hardware_interface REQUIRED) find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) @@ -23,7 +20,7 @@ find_package(pybind11 REQUIRED) add_subdirectory(cppparser) -set(hi_py_cpp ${CMAKE_CURRENT_BINARY_DIR}/src/hardware_interface_py.cpp) +set(hi_py_cpp ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface_py.cpp) add_executable(ros2_control_py_builder src/ros2_control_py_builder/main.cpp @@ -34,27 +31,23 @@ target_link_libraries(ros2_control_py_builder ) add_custom_command( OUTPUT ${hi_py_cpp} - COMMAND ros2_control_py_builder /opt/ros/humble/include/hardware_interface ${CMAKE_CURRENT_BINARY_DIR} + COMMAND ros2_control_py_builder /opt/ros/$ENV{ROS_DISTRO}/include/hardware_interface ${CMAKE_CURRENT_BINARY_DIR} DEPENDS ros2_control_py_builder ) ament_python_install_package(ros2_control_py) -pybind11_add_module(ros2_control_py_core +pybind11_add_module(hardware_interface_py ${hi_py_cpp} ) -ament_target_dependencies(ros2_control_py_core +ament_target_dependencies(hardware_interface_py PUBLIC rclcpp rclcpp_lifecycle hardware_interface ) -target_include_directories(ros2_control_py_core - PRIVATE - ${CMAKE_CURRENT_BINARY_DIR}/include -) install( - TARGETS ros2_control_py_core + TARGETS hardware_interface_py DESTINATION "${PYTHON_INSTALL_DIR}/ros2_control_py" ) diff --git a/README.md b/README.md index abb832b..4ae6a5b 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,31 @@ # Python bindings for ros2_control using pybind11 + ------------------------------------------------- + +## Install + +Assuming that your [ros2_control](https://github.com/ros-controls/ros2_control) workspace is ìn the directory `~/ros2_control_py_ws`: + +```sh +mkdir -p ~/ros2_control_py_ws/src +cd ~/ros2_control_py_ws/src +git clone https://github.com/Gepetto/ros2_control_py --recursive +cd ~/ros2_control_py_ws +rosdep install --from-paths src -y --ignore-src +``` + +## Compiling + +```sh +cd ~/ros2_control_py_ws +colcon build --packages-select ros2_control_py +``` + +## Testing + +To create an actuator + +```python +from ros2_control_py import Actuator +actuator = Actuator() +``` diff --git a/ros2_control_py/__init__.py b/ros2_control_py/__init__.py index 2c550fe..fd81c2a 100644 --- a/ros2_control_py/__init__.py +++ b/ros2_control_py/__init__.py @@ -1 +1 @@ -from ros2_control_py.core import * +from .hardware_interface_py import * diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index b4cd928..2fb3b57 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -7,16 +7,16 @@ int main(int argc, char** argv) { ASSERT(argc == 3, "Invalid number of command line arguments, expected 2 got " << argc - 1); - sfs::path hi_dir = argv[1]; - sfs::path dst_dir = argv[2]; - ASSERT(sfs::is_directory(hi_dir), hi_dir << " is not a valid directory"); - ASSERT(sfs::is_directory(dst_dir), dst_dir << " is not a valid file"); + fs::path hi_dir = argv[1]; + fs::path dst_dir = argv[2]; + ASSERT(fs::is_directory(hi_dir), hi_dir << " is not a valid directory"); + ASSERT(fs::is_directory(dst_dir), dst_dir << " is not a valid file"); std::vector
headers; - for (auto entry : sfs::recursive_directory_iterator{hi_dir}) { - const sfs::path& path = entry.path(); - if (!entry.is_regular_file() || path.extension() != ".hpp" || + for (auto entry : fs::recursive_directory_iterator{hi_dir}) { + const fs::path& path = entry.path(); + if (!fs::is_regular_file(entry) || path.extension() != ".hpp" || path.filename() == "macros.hpp" || path.filename() == "component_parser.hpp") continue; @@ -25,13 +25,13 @@ int main(int argc, char** argv) { parse_header(headers, path, name); } - sfs::path src_dir = dst_dir / "src"; - sfs::path hi_py = src_dir / "hardware_interface_py.cpp"; - sfs::path inc_hi_dir = dst_dir / "include" / "hardware_interface"; - sfs::path inc_hi_types_dir = inc_hi_dir / "types"; + fs::path src_dir = dst_dir / "src" / "ros2_control_py"; + fs::path hi_py = src_dir / "hardware_interface_py.cpp"; + fs::path inc_hi_dir = src_dir / "hardware_interface"; + fs::path inc_hi_types_dir = inc_hi_dir / "types"; - sfs::create_directories(src_dir); - sfs::create_directories(inc_hi_types_dir); + fs::create_directories(src_dir); + fs::create_directories(inc_hi_types_dir); for (const Header& header : headers) write_named_hi_py_hpp(inc_hi_dir, header); diff --git a/src/ros2_control_py_builder/parse.hpp b/src/ros2_control_py_builder/parse.hpp index bce840d..58e2e6a 100644 --- a/src/ros2_control_py_builder/parse.hpp +++ b/src/ros2_control_py_builder/parse.hpp @@ -2,7 +2,8 @@ // STL #include -#include +// boost +#include // CppParser #include #include @@ -15,7 +16,7 @@ #include "structs.hpp" #include "utils.hpp" -namespace sfs = std::filesystem; +namespace fs = boost::filesystem; inline void parse_class_attr(Cls& cls, CppConstVarEPtr attr) { cls.attrs.emplace_back(attr->name()); @@ -144,7 +145,7 @@ inline void parse_namespace(Header& header, CppWriter& writer, } } -inline void parse_header(std::vector
& headers, sfs::path path, +inline void parse_header(std::vector
& headers, fs::path path, const std::string& name) { CppParser parser; parser.addIgnorableMacro("HARDWARE_INTERFACE_PUBLIC"); diff --git a/src/ros2_control_py_builder/structs.hpp b/src/ros2_control_py_builder/structs.hpp index 5aa59a6..c069f1b 100644 --- a/src/ros2_control_py_builder/structs.hpp +++ b/src/ros2_control_py_builder/structs.hpp @@ -52,11 +52,15 @@ struct Cls { }; struct Enum { + Enum(const std::string& name) : name{name} {}; + std::string name; std::vector items; }; struct Var { + Var(const std::string& name) : name{name} {}; + std::string name; }; diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index 70c6183..6d6cf60 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -1,13 +1,14 @@ #pragma once // STL -#include #include +// boost +#include // ros2_control_pŷ_builder #include "structs.hpp" #include "utils.hpp" -namespace sfs = std::filesystem; +namespace fs = boost::filesystem; /// @brief output a `Cls` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Cls& cls); @@ -18,17 +19,17 @@ inline std::ostream& operator<<(std::ostream& os, const Enum& enu); /// @brief output a `Var` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Var& var); /// @brief write a hpp file in `inc_hi_dir` for header `header` -inline void write_named_hi_py_hpp(const sfs::path& inc_hi_dir, +inline void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const Header& header); /// @brief write a cpp file in `hi_py` calling bindings from headers `headers` -inline void write_hi_py_cpp(const sfs::path& hi_py, +inline void write_hi_py_cpp(const fs::path& hi_py, const std::vector
& headers); // Impl inline std::ostream& operator<<(std::ostream& os, const Cls& cls) { if (!cls.vmembs.empty()) { - Cls py_cls{"Py" + cls.name, cls.name}; + Cls py_cls{cls.name, "Py" + cls.name}; py_cls.attrs = cls.attrs; py_cls.ctors = cls.ctors; py_cls.membs = cls.membs; @@ -85,8 +86,8 @@ inline std::ostream& operator<<(std::ostream& os, const Var& var) { << "\", []() { return std::string{" << var.name << "}; });"; } -void write_named_hi_py_hpp(const sfs::path& inc_hi_dir, const Header& header) { - sfs::path path = inc_hi_dir / (header.name + "_py.hpp"); +void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const Header& header) { + fs::path path = inc_hi_dir / (header.name + "_py.hpp"); std::ofstream ofs{path, std::ios::out | std::ios::trunc}; ASSERT(ofs, "Could not open " << path); ofs << R"(// pybind11 @@ -105,7 +106,7 @@ namespace py = pybind11; for (const Cls& cls : header.classes) { if (cls.vmembs.empty()) continue; ofs << "\nclass Py" << cls.name << ": public " << cls.name - << "{\n public:\n using " << cls.name << "::" << cls.name << ";\n"; + << " {\n public:\n using " << cls.name << "::" << cls.name << ";\n"; for (const VMemb& vmemb : cls.vmembs) ofs << vmemb; ofs << "};\n"; } @@ -122,7 +123,7 @@ inline void init_)" )"; } -void write_hi_py_cpp(const sfs::path& hi_py, +void write_hi_py_cpp(const fs::path& hi_py, const std::vector
& headers) { std::ofstream ofs{hi_py, std::ios::out | std::ios::trunc}; ASSERT(ofs, "could not open " << hi_py); @@ -134,7 +135,7 @@ void write_hi_py_cpp(const sfs::path& hi_py, // hardware_interface_py )"; for (const Header& header : headers) - ofs << "#include \n"; + ofs << "#include \"hardware_interface/" << header.name << "_py.hpp\"\n"; ofs << R"( namespace py = pybind11; From 9a0565349856dc7adf1ea33d17d4bbcbac2844d0 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 4 Oct 2023 11:15:23 +0200 Subject: [PATCH 14/42] feat: removed digit separators from sources to compile on rolling --- src/ros2_control_py_builder/utils.hpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/ros2_control_py_builder/utils.hpp b/src/ros2_control_py_builder/utils.hpp index f824133..6c5ed21 100644 --- a/src/ros2_control_py_builder/utils.hpp +++ b/src/ros2_control_py_builder/utils.hpp @@ -2,8 +2,10 @@ // STL #include +#include #include #include +#include #include #include // CppParser @@ -43,12 +45,21 @@ inline std::string str_of_cpp(CppWriter& writer, const CppObj* cppObj); // Impl inline void remove_attributes(std::string& contents) { + // remove attributes aka [[...]] auto it = contents.begin(); while (it != contents.end()) { it = std::search_n(it, contents.end(), 2, '['); auto end = std::search_n(it, contents.end(), 2, ']'); if (end != contents.end()) it = contents.erase(it, end + 2); } + // digit separators aka d'ddd'ddd + it = contents.begin(); + while (it != contents.end()) { + it = std::adjacent_find(it, contents.end(), [](char a, char b) { + return std::isdigit(a) && b == '\''; + }); + if (it != contents.end()) it = contents.erase(it + 1); + } } inline std::string read_file(const std::string& filename) { From 133bca705d30dd9406ac6bf9dac9cdc1eb745624 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 4 Oct 2023 13:59:25 +0200 Subject: [PATCH 15/42] feat: auto create new dirs --- src/ros2_control_py_builder/main.cpp | 2 -- src/ros2_control_py_builder/write.hpp | 2 ++ 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index 2fb3b57..a3158c1 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -28,10 +28,8 @@ int main(int argc, char** argv) { fs::path src_dir = dst_dir / "src" / "ros2_control_py"; fs::path hi_py = src_dir / "hardware_interface_py.cpp"; fs::path inc_hi_dir = src_dir / "hardware_interface"; - fs::path inc_hi_types_dir = inc_hi_dir / "types"; fs::create_directories(src_dir); - fs::create_directories(inc_hi_types_dir); for (const Header& header : headers) write_named_hi_py_hpp(inc_hi_dir, header); diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index 6d6cf60..4e8a1bf 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -1,6 +1,7 @@ #pragma once // STL +#include #include // boost #include @@ -88,6 +89,7 @@ inline std::ostream& operator<<(std::ostream& os, const Var& var) { void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const Header& header) { fs::path path = inc_hi_dir / (header.name + "_py.hpp"); + fs::create_directory(path.parent_path()); std::ofstream ofs{path, std::ios::out | std::ios::trunc}; ASSERT(ofs, "Could not open " << path); ofs << R"(// pybind11 From 42aab8f09cc722c6d0fde2541b59516c3c37ae0a Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 4 Oct 2023 14:01:16 +0200 Subject: [PATCH 16/42] fix: moved built files to root of src --- CMakeLists.txt | 2 +- src/ros2_control_py_builder/main.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ca1a4e4..b210612 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(pybind11 REQUIRED) add_subdirectory(cppparser) -set(hi_py_cpp ${CMAKE_CURRENT_BINARY_DIR}/src/ros2_control_py/hardware_interface_py.cpp) +set(hi_py_cpp ${CMAKE_CURRENT_BINARY_DIR}/src/hardware_interface_py.cpp) add_executable(ros2_control_py_builder src/ros2_control_py_builder/main.cpp diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index a3158c1..e391c7c 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -25,7 +25,7 @@ int main(int argc, char** argv) { parse_header(headers, path, name); } - fs::path src_dir = dst_dir / "src" / "ros2_control_py"; + fs::path src_dir = dst_dir / "src"; fs::path hi_py = src_dir / "hardware_interface_py.cpp"; fs::path inc_hi_dir = src_dir / "hardware_interface"; From 3bda488e55e518ecbf18fe81770544766756f18f Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 4 Oct 2023 14:03:47 +0200 Subject: [PATCH 17/42] fix: mkdirs instead of mkdir --- src/ros2_control_py_builder/write.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index 4e8a1bf..49fc2f7 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -89,7 +89,7 @@ inline std::ostream& operator<<(std::ostream& os, const Var& var) { void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const Header& header) { fs::path path = inc_hi_dir / (header.name + "_py.hpp"); - fs::create_directory(path.parent_path()); + fs::create_directories(path.parent_path()); std::ofstream ofs{path, std::ios::out | std::ios::trunc}; ASSERT(ofs, "Could not open " << path); ofs << R"(// pybind11 From 3035be97d93aaf84299df5404738aa303a28a68e Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 4 Oct 2023 14:05:27 +0200 Subject: [PATCH 18/42] feat: create src/hi dir --- src/ros2_control_py_builder/main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index e391c7c..0b25027 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -27,12 +27,12 @@ int main(int argc, char** argv) { fs::path src_dir = dst_dir / "src"; fs::path hi_py = src_dir / "hardware_interface_py.cpp"; - fs::path inc_hi_dir = src_dir / "hardware_interface"; + fs::path src_hi_dir = src_dir / "hardware_interface"; - fs::create_directories(src_dir); + fs::create_directories(src_hi_dir); for (const Header& header : headers) - write_named_hi_py_hpp(inc_hi_dir, header); + write_named_hi_py_hpp(src_hi_dir, header); write_hi_py_cpp(hi_py, headers); From 2003b56db9a8dc3f2b4511fd6e004a86415c683e Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 6 Oct 2023 11:01:23 +0200 Subject: [PATCH 19/42] feat: added possibility to bind more modules than juste hardware_interface --- CMakeLists.txt | 48 +++++++++----- ros2_control_py/__init__.py | 12 +++- src/ros2_control_py_builder/main.cpp | 53 ++++++++------- src/ros2_control_py_builder/parse.hpp | 24 +++++-- src/ros2_control_py_builder/structs.hpp | 48 +++++++++++--- src/ros2_control_py_builder/utils.hpp | 18 +++++- src/ros2_control_py_builder/write.hpp | 85 ++++++++++++------------- 7 files changed, 186 insertions(+), 102 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b210612..736dbfe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,9 +1,18 @@ cmake_minimum_required(VERSION 3.5) + project(ros2_control_py) +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_EXTENSIONS OFF) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() +if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) endif() find_package(ament_cmake REQUIRED) @@ -20,7 +29,11 @@ find_package(pybind11 REQUIRED) add_subdirectory(cppparser) -set(hi_py_cpp ${CMAKE_CURRENT_BINARY_DIR}/src/hardware_interface_py.cpp) +set(ros2_py_src_dir ${CMAKE_CURRENT_BINARY_DIR}/src) +set(ros2_py_modules hardware_interface) +set(ros2_py_src ${ros2_py_modules}) +list(TRANSFORM ros2_py_src PREPEND ${ros2_py_src_dir}/) +list(TRANSFORM ros2_py_src APPEND _py.cpp) add_executable(ros2_control_py_builder src/ros2_control_py_builder/main.cpp @@ -30,25 +43,26 @@ target_link_libraries(ros2_control_py_builder cppparser ) add_custom_command( - OUTPUT ${hi_py_cpp} - COMMAND ros2_control_py_builder /opt/ros/$ENV{ROS_DISTRO}/include/hardware_interface ${CMAKE_CURRENT_BINARY_DIR} + OUTPUT ${ros2_py_src} + COMMAND ros2_control_py_builder "${CMAKE_CURRENT_BINARY_DIR}" "/opt/ros/$ENV{ROS_DISTRO}/include" ${ros2_py_modules} DEPENDS ros2_control_py_builder ) ament_python_install_package(ros2_control_py) -pybind11_add_module(hardware_interface_py - ${hi_py_cpp} -) -ament_target_dependencies(hardware_interface_py - PUBLIC - rclcpp - rclcpp_lifecycle - hardware_interface -) -install( - TARGETS hardware_interface_py - DESTINATION "${PYTHON_INSTALL_DIR}/ros2_control_py" -) +foreach(module ${ros2_py_modules}) + pybind11_add_module(${module} + ${ros2_py_src} + ) + ament_target_dependencies(${module} PUBLIC + rclpy + rclcpp + rclcpp_lifecycle + hardware_interface + ) + install(TARGETS ${module} + DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" + ) +endforeach() ament_package() diff --git a/ros2_control_py/__init__.py b/ros2_control_py/__init__.py index fd81c2a..db95cd0 100644 --- a/ros2_control_py/__init__.py +++ b/ros2_control_py/__init__.py @@ -1 +1,11 @@ -from .hardware_interface_py import * +import pkgutil + +__all__ = [] +for loader, module_name, is_pkg in pkgutil.walk_packages(__path__): + __all__.append(module_name) + globals()[module_name] = loader.find_module(module_name).load_module(module_name) + +del pkgutil +del loader +del module_name +del is_pkg diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index 0b25027..4a2a627 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -4,37 +4,42 @@ #include "write.hpp" int main(int argc, char** argv) { - ASSERT(argc == 3, "Invalid number of command line arguments, expected 2 got " - << argc - 1); + ASSERT(argc > 3, + "Invalid number of command line arguments, expected at least 3 got " + << argc - 1); - fs::path hi_dir = argv[1]; - fs::path dst_dir = argv[2]; - ASSERT(fs::is_directory(hi_dir), hi_dir << " is not a valid directory"); - ASSERT(fs::is_directory(dst_dir), dst_dir << " is not a valid file"); + fs::path dst_dir = argv[1]; + fs::path inc_dir = argv[2]; + fs::path src_dir = dst_dir / "src"; + fs::create_directories(src_dir); - std::vector
headers; + ASSERT_DIR(src_dir); + ASSERT_DIR(inc_dir); - for (auto entry : fs::recursive_directory_iterator{hi_dir}) { - const fs::path& path = entry.path(); - if (!fs::is_regular_file(entry) || path.extension() != ".hpp" || - path.filename() == "macros.hpp" || - path.filename() == "component_parser.hpp") - continue; + std::vector modules; + for (int i = 3; i < argc; ++i) + modules.emplace_back(inc_dir, src_dir, argv[i]); - std::string name = path.lexically_relative(hi_dir).string(); - parse_header(headers, path, name); + for (const Module& mod : modules) { + fs::create_directories(mod.src_dir); + ASSERT_DIR(mod.src_dir); + ASSERT_DIR(mod.inc_dir); } - fs::path src_dir = dst_dir / "src"; - fs::path hi_py = src_dir / "hardware_interface_py.cpp"; - fs::path src_hi_dir = src_dir / "hardware_interface"; - - fs::create_directories(src_hi_dir); - - for (const Header& header : headers) - write_named_hi_py_hpp(src_hi_dir, header); + for (Module& mod : modules) { + for (const fs::directory_entry& entry : + fs::recursive_directory_iterator{mod.inc_dir}) { + const fs::path& path = entry.path(); + if (!fs::is_regular_file(entry) || path.extension() != ".hpp" || + path.filename() == "macros.hpp") + continue; + + std::string name = path.lexically_relative(mod.inc_dir).string(); + parse_header(mod, path, name); + } + } - write_hi_py_cpp(hi_py, headers); + for (const Module& mod : modules) write_module(mod); return 0; } diff --git a/src/ros2_control_py_builder/parse.hpp b/src/ros2_control_py_builder/parse.hpp index 58e2e6a..27ed593 100644 --- a/src/ros2_control_py_builder/parse.hpp +++ b/src/ros2_control_py_builder/parse.hpp @@ -122,6 +122,10 @@ inline void parse_var(Header& header, CppVarEPtr var) { header.vars.emplace_back(var->name()); } +inline void parse_func(Header& header, CppFunctionEPtr func) { + header.funcs.emplace_back(func->name_); +} + inline void parse_namespace(Header& header, CppWriter& writer, CppConstCompoundEPtr ns, std::string name) { header.namespaces.emplace_back(name); @@ -131,6 +135,11 @@ inline void parse_namespace(Header& header, CppWriter& writer, parse_var(header, var); continue; } + CppFunctionEPtr func = obj; + if (func) { + parse_func(header, func); + continue; + } CppEnumEPtr enu = obj; if (enu && enu->itemList_) { parse_enum(header, enu); @@ -145,20 +154,23 @@ inline void parse_namespace(Header& header, CppWriter& writer, } } -inline void parse_header(std::vector
& headers, fs::path path, - const std::string& name) { +inline void parse_header(Module& mod, fs::path path, const std::string& name) { CppParser parser; - parser.addIgnorableMacro("HARDWARE_INTERFACE_PUBLIC"); + std::string const upper_name = to_upper(mod.name); + parser.addIgnorableMacro(upper_name + "_PUBLIC"); + parser.addIgnorableMacro(upper_name + "_LOCAL"); + parser.addIgnorableMacro(upper_name + "_EXPORT"); + parser.addIgnorableMacro(upper_name + "_IMPORT"); CppWriter writer; - headers.emplace_back(name.substr(0, name.rfind('.'))); + mod.headers.emplace_back(name.substr(0, name.rfind('.'))); const CppCompoundPtr ast = parse_file(parser, path.string()); ASSERT(ast, "Could not parse " << path); // std::cerr << "Parsed " << path << std::endl; for (const CppObjPtr& obj_ns : ast->members()) { CppConstCompoundEPtr ns = obj_ns; - if (!ns || !isNamespace(ns) || ns->name() != "hardware_interface") continue; - parse_namespace(headers.back(), writer, ns, ns->name()); + if (!ns || !isNamespace(ns) || ns->name() != mod.name) continue; + parse_namespace(mod.headers.back(), writer, ns, ns->name()); } } diff --git a/src/ros2_control_py_builder/structs.hpp b/src/ros2_control_py_builder/structs.hpp index c069f1b..b691314 100644 --- a/src/ros2_control_py_builder/structs.hpp +++ b/src/ros2_control_py_builder/structs.hpp @@ -3,6 +3,10 @@ // STL #include #include +// boost +#include + +namespace fs = boost::filesystem; struct Attr { Attr(const std::string& name) : name{name} {} @@ -35,16 +39,16 @@ struct VMemb { std::string name; std::string ret_type; std::string cls; - std::vector args; - std::vector args_names; + std::vector const args; + std::vector const args_names; }; struct Cls { Cls(const std::string& name, const std::string& mother) : name{name}, mother{mother} {} - std::string name; - std::string mother; + std::string const name; + std::string const mother; std::vector attrs; std::vector membs; std::vector ctors; @@ -54,22 +58,50 @@ struct Cls { struct Enum { Enum(const std::string& name) : name{name} {}; - std::string name; + std::string const name; std::vector items; }; struct Var { Var(const std::string& name) : name{name} {}; - std::string name; + std::string const name; +}; + +struct Func { + Func(const std::string& name) : name{name} {}; + + std::string const name; }; struct Header { - Header(const std::string& name) : name{name} {} + Header(const std::string& name) + : name{name}, proper_name{get_proper_name(name)} {} - std::string name; + static std::string get_proper_name(std::string name) { + std::replace(name.begin(), name.end(), '/', '_'); + return name; + } + + std::string const name; + std::string const proper_name; std::vector namespaces; std::vector classes; std::vector enums; std::vector vars; + std::vector funcs; +}; + +struct Module { + Module(fs::path inc_dir, fs::path src_dir, const std::string& name) + : inc_dir{inc_dir / name}, + src_dir{src_dir / name}, + src{src_dir / (name + "_py.cpp")}, + name{name} {} + + fs::path const inc_dir; + fs::path const src_dir; + fs::path const src; + std::string name; + std::vector
headers; }; diff --git a/src/ros2_control_py_builder/utils.hpp b/src/ros2_control_py_builder/utils.hpp index 6c5ed21..e6732f2 100644 --- a/src/ros2_control_py_builder/utils.hpp +++ b/src/ros2_control_py_builder/utils.hpp @@ -19,6 +19,9 @@ std::cerr << __VA_ARGS__ << std::endl; \ std::exit(EXIT_FAILURE); \ } while (false) +/// @brief if not fs::is_directori((Dir)) fails +#define ASSERT_DIR(Dir) \ + ASSERT(fs::is_directory((Dir)), (Dir) << " is not a valid directory") /// @brief wrapper over a `const T&` and a `const U&`, display all elements of a /// `T` with a 'U' as a separator @@ -41,6 +44,8 @@ template std::ostream& operator<<(std::ostream& os, const Sep& sep); /// @brief wrapper to get a string repr of a CppObj inline std::string str_of_cpp(CppWriter& writer, const CppObj* cppObj); +/// @brief std::string to upper (not in place) +inline std::string to_upper(std::string str); // Impl @@ -52,13 +57,16 @@ inline void remove_attributes(std::string& contents) { auto end = std::search_n(it, contents.end(), 2, ']'); if (end != contents.end()) it = contents.erase(it, end + 2); } - // digit separators aka d'ddd'ddd + // digit separators aka d'ddd'ddd but not char digits aka 'd' it = contents.begin(); while (it != contents.end()) { it = std::adjacent_find(it, contents.end(), [](char a, char b) { return std::isdigit(a) && b == '\''; }); - if (it != contents.end()) it = contents.erase(it + 1); + if (it == contents.end()) continue; + it += 2; + if (it == contents.end() || !std::isdigit(*it)) continue; + it = contents.erase(it - 1); } } @@ -119,3 +127,9 @@ inline std::string str_of_cpp(CppWriter& writer, const CppObj* cppObj) { writer.emit(cppObj, oss); return std::move(oss).str(); } + +inline std::string to_upper(std::string str) { + std::transform(str.cbegin(), str.cend(), str.begin(), + [](char c) { return std::toupper(c); }); + return str; +} diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index 49fc2f7..55a98b6 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -1,7 +1,6 @@ #pragma once // STL -#include #include // boost #include @@ -19,12 +18,12 @@ inline std::ostream& operator<<(std::ostream& os, const VMemb& vmemb); inline std::ostream& operator<<(std::ostream& os, const Enum& enu); /// @brief output a `Var` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Var& var); -/// @brief write a hpp file in `inc_hi_dir` for header `header` -inline void write_named_hi_py_hpp(const fs::path& inc_hi_dir, - const Header& header); -/// @brief write a cpp file in `hi_py` calling bindings from headers `headers` -inline void write_hi_py_cpp(const fs::path& hi_py, - const std::vector
& headers); +/// @brief output a `Func` to write an hpp +inline std::ostream& operator<<(std::ostream& os, const Func& func); +/// @brief write source files for module `mod` and header `header` +inline void write_module_header(const Module& mod, const Header& header); +/// @brief write source files for module `mod` +inline void write_module(const Module& mod); // Impl @@ -39,8 +38,8 @@ inline std::ostream& operator<<(std::ostream& os, const Cls& cls) { return os << py_cls; } os << " py::class_<" << cls.name - << (cls.mother.empty() ? "" : ", " + cls.mother) - << ">(hardware_interface_py, \"" << cls.name << "\")"; + << (cls.mother.empty() ? "" : ", " + cls.mother) << ">(m, \"" << cls.name + << "\")"; std::vector ctors = cls.ctors; if (ctors.empty()) ctors.emplace_back(std::vector{}); for (const Ctor& ctor : ctors) @@ -74,8 +73,7 @@ inline std::ostream& operator<<(std::ostream& os, const VMemb& vmemb) { inline std::ostream& operator<<(std::ostream& os, const Enum& enu) { ASSERT(!enu.items.empty(), "empty enum"); - os << " py::enum_<" << enu.name << ">(hardware_interface_py, \"" << enu.name - << "\")\n"; + os << " py::enum_<" << enu.name << ">(m, \"" << enu.name << "\")\n"; for (const std::string& item : enu.items) os << " .value(\"" << item << "\", " << enu.name << "::" << item << ")\n"; @@ -83,22 +81,27 @@ inline std::ostream& operator<<(std::ostream& os, const Enum& enu) { } inline std::ostream& operator<<(std::ostream& os, const Var& var) { - return os << " hardware_interface_py.def(\"" << var.name - << "\", []() { return std::string{" << var.name << "}; });"; + return os << " m.def(\"" << var.name << "\", []() { return std::string{" + << var.name << "}; });\n"; } -void write_named_hi_py_hpp(const fs::path& inc_hi_dir, const Header& header) { - fs::path path = inc_hi_dir / (header.name + "_py.hpp"); - fs::create_directories(path.parent_path()); +inline std::ostream& operator<<(std::ostream& os, const Func& func) { + return os << " m.def(\"" << func.name << "\", &" << func.name << ");\n"; +} + +void write_module_header(const Module& mod, const Header& header) { + fs::path path = mod.src_dir / (header.name + "_py.hpp"); std::ofstream ofs{path, std::ios::out | std::ios::trunc}; ASSERT(ofs, "Could not open " << path); ofs << R"(// pybind11 #include -// hardware_interface -#include +// )" << mod.name + << R"( +#include <)" + << mod.name << R"(/)" << header.name << R"(.hpp> -namespace ros2_control_py::bind_hardware_interface +namespace ros2_control_py::bind_)" + << mod.name << R"( { namespace py = pybind11; @@ -112,36 +115,35 @@ namespace py = pybind11; for (const VMemb& vmemb : cls.vmembs) ofs << vmemb; ofs << "};\n"; } - std::string proper = header.name; - std::replace(proper.begin(), proper.end(), '/', '_'); ofs << R"( inline void init_)" - << proper << R"((py::module &hardware_interface_py) + << header.proper_name << R"((py::module &m) { )" << Sep(header.vars, "\n") - << Sep(header.enums, "\n") << Sep(header.classes, "\n") << R"(} + << Sep(header.funcs, "\n") << Sep(header.enums, "\n") + << Sep(header.classes, "\n") << R"(} } )"; } -void write_hi_py_cpp(const fs::path& hi_py, - const std::vector
& headers) { - std::ofstream ofs{hi_py, std::ios::out | std::ios::trunc}; - ASSERT(ofs, "could not open " << hi_py); +void write_module(const Module& mod) { + for (const Header& header : mod.headers) write_module_header(mod, header); + + std::ofstream ofs{mod.src, std::ios::out | std::ios::trunc}; + ASSERT(ofs, "could not open " << mod.src); ofs << R"(// pybind11 #include -// parse_control_resources_from_urdf -#include -// hardware_interface_py -)"; - for (const Header& header : headers) - ofs << "#include \"hardware_interface/" << header.name << "_py.hpp\"\n"; +// )" << mod.name + << '\n'; + for (const Header& header : mod.headers) + ofs << "#include \"" << mod.name << "/" << header.name << "_py.hpp\"\n"; ofs << R"( namespace py = pybind11; -PYBIND11_MODULE(hardware_interface_py, m) +PYBIND11_MODULE()" + << mod.name << R"(, m) { m.doc() = R"doc( Python bindings for ros2_control functionalities. @@ -153,14 +155,9 @@ PYBIND11_MODULE(hardware_interface_py, m) // Construct module classes )"; - for (const Header& header : headers) { - std::string proper = header.name; - std::replace(proper.begin(), proper.end(), '/', '_'); - ofs << " ros2_control_py::bind_hardware_interface::init_" << proper - << "(m);\n"; + for (const Header& header : mod.headers) { + ofs << " ros2_control_py::bind_" << mod.name << "::init_" + << header.proper_name << "(m);\n"; } - ofs << R"( - m.def("parse_control_resources_from_urdf", hardware_interface::parse_control_resources_from_urdf); -} -)"; + ofs << "}\n"; } From e44be6416610d3556489e0ff679f2e663ba06a4e Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Tue, 10 Oct 2023 18:29:54 +0200 Subject: [PATCH 20/42] feat: finished reworking everyting to make it work with more headers --- CMakeLists.txt | 26 ++- ros2_control_py/__init__.py | 6 +- src/ros2_control_py_builder/main.cpp | 125 ++++++++++++ src/ros2_control_py_builder/parse.hpp | 145 ++++++++----- src/ros2_control_py_builder/structs.hpp | 261 +++++++++++++++++++----- src/ros2_control_py_builder/utils.hpp | 146 ++++++++++++- src/ros2_control_py_builder/write.hpp | 122 ++++++----- 7 files changed, 668 insertions(+), 163 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 736dbfe..cd169d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,13 +16,7 @@ if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) find_package(rclpy REQUIRED) -#find_package(controller_interface REQUIRED) -#find_package(controller_manager REQUIRED) -#find_package(controller_manager_msgs REQUIRED) -find_package(hardware_interface REQUIRED) find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) @@ -30,7 +24,12 @@ find_package(pybind11 REQUIRED) add_subdirectory(cppparser) set(ros2_py_src_dir ${CMAKE_CURRENT_BINARY_DIR}/src) -set(ros2_py_modules hardware_interface) + +set(ros2_py_modules + hardware_interface + controller_interface +) + set(ros2_py_src ${ros2_py_modules}) list(TRANSFORM ros2_py_src PREPEND ${ros2_py_src_dir}/) list(TRANSFORM ros2_py_src APPEND _py.cpp) @@ -51,16 +50,15 @@ add_custom_command( ament_python_install_package(ros2_control_py) foreach(module ${ros2_py_modules}) - pybind11_add_module(${module} - ${ros2_py_src} + find_package(${module} REQUIRED) + pybind11_add_module(${module}_py + ${ros2_py_src_dir}/${module}_py.cpp ) - ament_target_dependencies(${module} PUBLIC + ament_target_dependencies(${module}_py PUBLIC rclpy - rclcpp - rclcpp_lifecycle - hardware_interface + ${module} ) - install(TARGETS ${module} + install(TARGETS ${module}_py DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" ) endforeach() diff --git a/ros2_control_py/__init__.py b/ros2_control_py/__init__.py index db95cd0..5ffeae5 100644 --- a/ros2_control_py/__init__.py +++ b/ros2_control_py/__init__.py @@ -2,8 +2,10 @@ __all__ = [] for loader, module_name, is_pkg in pkgutil.walk_packages(__path__): - __all__.append(module_name) - globals()[module_name] = loader.find_module(module_name).load_module(module_name) + __all__.append(module_name[:-3]) + globals()[module_name[:-3]] = loader.find_module(module_name).load_module( + module_name + ) del pkgutil del loader diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index 4a2a627..c9c37c2 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -1,8 +1,128 @@ +// STL +#include // ros2_control_py_builder #include "parse.hpp" #include "utils.hpp" #include "write.hpp" +void init_cls_mothers(Module& mod) { + std::size_t size = 0; + for (Header& header : ptr_iter(mod.headers)) size += header.classes.size(); + std::unordered_map> classes; + classes.reserve(size); + + for (Header& header : ptr_iter(mod.headers)) + for (std::shared_ptr cls : header.classes) + ASSERT(classes.insert({cls->name, cls}).second, + "Duplicate class " << cls->name << " in " << header.name); + + for (Header& header : ptr_iter(mod.headers)) { + for (Cls& cls : ptr_iter(header.classes)) { + if (cls.init) continue; + auto it = classes.find(cls.mother_just_name); + if (it == classes.end()) { + std::cerr << cls.name << " did not find " << cls.mother_just_name + << std::endl; + cls.init = true; + continue; + } + cls.mother = it->second; + if (header.name != cls.mother->header.name) + header.required.emplace_back(cls.mother->header.name); + if (!cls.using_mother_ctor) continue; + for (const Ctor& ctor : cls.mother->ctors) + if (std::find(cls.ctors.cbegin(), cls.ctors.cend(), ctor) == + cls.ctors.cend()) + cls.ctors.emplace_back(ctor); + } + } +} + +void init_cls_vmembs(Module& mod) { + bool did_all = false; + while (!did_all) { + did_all = true; + for (Header& header : ptr_iter(mod.headers)) { + for (Cls& cls : ptr_iter(header.classes)) { + if (cls.init) continue; + if (!cls.mother->init) { + did_all = false; + continue; + } + cls.init = true; + if (!cls.mother->has_virtual) continue; + for (Memb const& vmemb : ptr_iter(cls.mother->find_vmembs())) { + auto ovrd = cls.find_override(vmemb); + if (ovrd) { + if (!ovrd->is_virtual) ovrd->is_virtual = true; + continue; + } + cls.membs.emplace_back(std::make_shared(vmemb.clone(cls.name))); + cls.has_virtual = true; + if (vmemb.is_pure) cls.has_pure = true; + if (!vmemb.is_public) cls.has_protected = true; + } + } + } + } +} + +void init_overloads(Module& mod) { + for (Header& header : ptr_iter(mod.headers)) { + for (Func const& func : ptr_iter(header.funcs)) { + if (func.is_overloaded) continue; + auto overloads = header.find_overloads(func.name); + if (overloads.size() <= 1) continue; + for (Func& overload : ptr_iter(overloads)) overload.is_overloaded = true; + } + for (Cls& cls : ptr_iter(header.classes)) { + std::unordered_set> to_remove; + for (Memb const& memb : ptr_iter(cls.membs)) { + if (memb.is_overloaded) continue; + auto overloads = cls.find_overloads(memb.name); + if (overloads.size() <= 1) continue; + for (std::shared_ptr overload : overloads) { + overload->is_overloaded = true; + auto covrd = cls.find_mutable_overload(*overload); + if (covrd) to_remove.insert(overload); + } + } + cls.membs.erase(std::remove_if(cls.membs.begin(), cls.membs.end(), + [&to_remove](std::shared_ptr memb) { + return to_remove.find(memb) != + to_remove.end(); + }), + cls.membs.end()); + } + } +} + +void init_header_order(Module& mod) { + std::vector> headers; + auto old_headers = ptr_iter(mod.headers); + while (!mod.headers.empty()) { + for (auto it = old_headers.begin(); it != old_headers.end();) { + bool valid = true; + for (const std::string& req : it->required) { + if (std::find_if(headers.cbegin(), headers.cend(), + [req](std::shared_ptr other) { + return other->name == req; + }) == headers.cend()) { + valid = false; + break; + } + } + if (!valid) { + ++it; + continue; + } + headers.emplace_back(std::move(*it.base())); + it.from_base(mod.headers.erase(it.base())); + } + } + mod.headers = std::move(headers); +} + int main(int argc, char** argv) { ASSERT(argc > 3, "Invalid number of command line arguments, expected at least 3 got " @@ -37,6 +157,11 @@ int main(int argc, char** argv) { std::string name = path.lexically_relative(mod.inc_dir).string(); parse_header(mod, path, name); } + + init_cls_mothers(mod); + init_cls_vmembs(mod); + init_overloads(mod); + init_header_order(mod); } for (const Module& mod : modules) write_module(mod); diff --git a/src/ros2_control_py_builder/parse.hpp b/src/ros2_control_py_builder/parse.hpp index 27ed593..6ce60e2 100644 --- a/src/ros2_control_py_builder/parse.hpp +++ b/src/ros2_control_py_builder/parse.hpp @@ -1,7 +1,5 @@ #pragma once -// STL -#include // boost #include // CppParser @@ -19,7 +17,14 @@ namespace fs = boost::filesystem; inline void parse_class_attr(Cls& cls, CppConstVarEPtr attr) { - cls.attrs.emplace_back(attr->name()); + if (attr->varType()->typeModifier().refType_ != CppRefType::kNoRef) { + std::cerr << "skipped ref attr " << cls.name << "::" << attr->name() + << std::endl; + return; + } + if (!isPublic(attr)) return; + cls.attrs.emplace_back(std::make_shared(attr->name(), isPublic(attr))); + if (!cls.attrs.back()->is_public) cls.has_protected = true; } inline void parse_class_ctor(Cls& cls, CppConstructorEPtr ctor, @@ -45,37 +50,48 @@ inline void parse_class_ctor(Cls& cls, CppConstructorEPtr ctor, inline void parse_class_memb(Cls& cls, CppConstFunctionEPtr memb, CppWriter& writer) { - { - if (isPureVirtual(memb.get())) { - std::vector args; - std::vector args_names; - const CppParamVector* params = memb->params(); - if (params) { - for (const CppObjPtr& param : *params) { - CppConstVarEPtr var = param; - ASSERT(var, "that was not a var"); - std::string arg = str_of_cpp(writer, var); - arg.pop_back(); - arg.pop_back(); - args.emplace_back(std::move(arg)); - args_names.emplace_back(var->name()); - } + if (memb->templateParamList()) { + std::cerr << cls.name << ": skipped template member " << memb->name_ + << std::endl; + return; + } + std::string ret_type = str_of_cpp(writer, memb->retType_.get()); + std::vector args; + std::vector args_type; + std::vector args_names; + const CppParamVector* params = memb->params(); + if (params) { + size_t i = 0; + for (const CppObjPtr& param : *params) { + CppConstVarEPtr var = param; + ASSERT(var, "that was not a var"); + /*if (var->varType()->typeModifier().refType_ == CppRefType::kByRef) + std::cerr << "warning: arg by ref in " << cls.name + << "::" << memb->name_ << std::endl;*/ + if (var->varType()->typeModifier().refType_ == CppRefType::kRValRef) { + std::cerr << "warning: skipped " << cls.name << "::" << memb->name_ + << " because of rvalue argument" << std::endl; + return; } - cls.vmembs.emplace_back(memb->name_, - str_of_cpp(writer, memb->retType_.get()), - cls.name, std::move(args), std::move(args_names)); - } else - cls.membs.emplace_back(memb->name_); + std::string type = str_of_cpp(writer, var->varType()); + std::string name = + !var->name().empty() ? var->name() : "arg" + std::to_string(i++); + args.emplace_back(type + " " + name); + args_type.emplace_back(std::move(type)); + args_names.emplace_back(std::move(name)); + } } + cls.membs.emplace_back(std::make_shared( + memb->name_, cls.name, ret_type, std::move(args), std::move(args_type), + std::move(args_names), isConst(memb.get()), isVirtual(memb.get()), + isPureVirtual(memb.get()), isFinal(memb.get()), isPublic(memb.get()))); + if (cls.membs.back()->is_virtual && !cls.membs.back()->is_final) + cls.has_virtual = true; + if (cls.membs.back()->is_pure) cls.has_pure = true; + if (!cls.membs.back()->is_public) cls.has_protected = true; } -inline void parse_class_using(Cls& cls, Header& header) { - auto it = std::find_if( - header.classes.begin(), header.classes.end(), - [cls](const Cls& other) { return other.name == cls.mother; }); - ASSERT(it != header.classes.end(), "Could not find parent class"); - cls.ctors.insert(cls.ctors.end(), it->ctors.begin(), it->ctors.end()); -} +inline void parse_class_using(Cls& cls) { cls.using_mother_ctor = true; } inline void parse_class(Header& header, CppWriter& writer, CppConstCompoundEPtr cls) { @@ -84,29 +100,32 @@ inline void parse_class(Header& header, CppWriter& writer, "Too many parents for " << cls->name()); bool has_mother = parents && !parents->empty() && parents->front().inhType == CppAccessType::kPublic; - Cls& cls_rep = header.classes.emplace_back( - cls->name(), has_mother ? parents->front().baseName : ""); + auto cls_rep = std::make_shared( + header, cls->name(), has_mother ? parents->front().baseName : ""); + header.classes.emplace_back(cls_rep); for (const CppObjPtr& obj_memb : cls->members()) { - if (!isPublic(obj_memb)) continue; + if (!isPublic(obj_memb) && !isProtected(obj_memb)) continue; CppConstVarEPtr attr = obj_memb; if (attr) { - parse_class_attr(cls_rep, attr); - continue; - } - CppConstructorEPtr ctor = obj_memb; - if (ctor && !ctor->isCopyConstructor() && !ctor->isMoveConstructor()) { - parse_class_ctor(cls_rep, ctor, writer); + parse_class_attr(*cls_rep, attr); continue; } CppConstFunctionEPtr memb = obj_memb; if (memb && memb->name_.find("operator") == std::string::npos && memb->name_ != "get_full_name" && memb->name_ != "import_component") { - parse_class_memb(cls_rep, memb, writer); + parse_class_memb(*cls_rep, memb, writer); + continue; + } + if (!isPublic(obj_memb)) continue; + CppConstructorEPtr ctor = obj_memb; + if (ctor && !ctor->isCopyConstructor() && !ctor->isMoveConstructor()) { + parse_class_ctor(*cls_rep, ctor, writer); continue; } CppConstUsingDeclEPtr use = obj_memb; - if (use && use->name_ == cls_rep.mother + "::" + cls_rep.mother) { - parse_class_using(cls_rep, header); + if (use && use->name_ == cls_rep->mother_just_name + + "::" + cls_rep->mother_just_name) { + parse_class_using(*cls_rep); continue; } } @@ -119,11 +138,42 @@ inline void parse_enum(Header& header, CppEnumEPtr enu) { } inline void parse_var(Header& header, CppVarEPtr var) { + if (var->templateParamList()) { + std::cerr << header.name << ": skipped template var " << var->name() + << std::endl; + return; + } header.vars.emplace_back(var->name()); } -inline void parse_func(Header& header, CppFunctionEPtr func) { - header.funcs.emplace_back(func->name_); +inline void parse_func(Header& header, CppWriter& writer, + CppFunctionEPtr func) { + if (func->templateParamList()) { + std::cerr << header.name << ": skipped template function " << func->name_ + << std::endl; + return; + } + std::string ret_type = str_of_cpp(writer, func->retType_.get()); + std::vector args; + std::vector args_type; + std::vector args_names; + const CppParamVector* params = func->params(); + if (params) { + size_t i = 0; + for (const CppObjPtr& param : *params) { + CppConstVarEPtr var = param; + ASSERT(var, "that was not a var"); + std::string type = str_of_cpp(writer, var->varType()); + std::string name = + !var->name().empty() ? var->name() : "arg" + std::to_string(i++); + args.emplace_back(type + " " + name); + args_type.emplace_back(std::move(type)); + args_names.emplace_back(std::move(name)); + } + } + header.funcs.emplace_back( + std::make_shared(func->name_, ret_type, std::move(args), + std::move(args_type), std::move(args_names))); } inline void parse_namespace(Header& header, CppWriter& writer, @@ -137,7 +187,7 @@ inline void parse_namespace(Header& header, CppWriter& writer, } CppFunctionEPtr func = obj; if (func) { - parse_func(header, func); + parse_func(header, writer, func); continue; } CppEnumEPtr enu = obj; @@ -163,7 +213,8 @@ inline void parse_header(Module& mod, fs::path path, const std::string& name) { parser.addIgnorableMacro(upper_name + "_IMPORT"); CppWriter writer; - mod.headers.emplace_back(name.substr(0, name.rfind('.'))); + mod.headers.emplace_back( + std::make_shared
(name.substr(0, name.rfind('.')))); const CppCompoundPtr ast = parse_file(parser, path.string()); ASSERT(ast, "Could not parse " << path); @@ -171,6 +222,6 @@ inline void parse_header(Module& mod, fs::path path, const std::string& name) { for (const CppObjPtr& obj_ns : ast->members()) { CppConstCompoundEPtr ns = obj_ns; if (!ns || !isNamespace(ns) || ns->name() != mod.name) continue; - parse_namespace(mod.headers.back(), writer, ns, ns->name()); + parse_namespace(*mod.headers.back(), writer, ns, ns->name()); } } diff --git a/src/ros2_control_py_builder/structs.hpp b/src/ros2_control_py_builder/structs.hpp index b691314..a1647c6 100644 --- a/src/ros2_control_py_builder/structs.hpp +++ b/src/ros2_control_py_builder/structs.hpp @@ -1,95 +1,252 @@ #pragma once // STL +#include #include #include // boost #include +// ros2_control_py_builder +#include "utils.hpp" namespace fs = boost::filesystem; -struct Attr { - Attr(const std::string& name) : name{name} {} +struct Var { + Var(const std::string& name) : name{name} {}; std::string name; }; +struct Func { + Func(const std::string& name, const std::string& ret_type, + std::vector&& args, std::vector&& args_type, + std::vector&& args_names) + : name{name}, + ret_type{ret_type}, + args{std::move(args)}, + args_type{std::move(args_type)}, + args_names{std::move(args_names)} {} + + friend bool operator==(const Func& lhs, const Func& rhs) { + return lhs.name == rhs.name && lhs.args_type == rhs.args_type; + } + friend bool operator!=(const Func& lhs, const Func& rhs) { + return !(lhs == rhs); + } + + const std::string name; + const std::string ret_type; + const std::vector args; + const std::vector args_type; + const std::vector args_names; + bool is_overloaded{false}; +}; + +struct Attr : public Var { + Attr(const std::string& name, bool is_public) + : Var{name}, is_public{is_public} {} + + bool is_public; +}; + struct Ctor { Ctor(std::vector&& args) : args{std::move(args)} {} + friend bool operator==(const Ctor& lhs, const Ctor& rhs) { + return lhs.args == rhs.args; + } + friend bool operator!=(const Ctor& lhs, const Ctor& rhs) { + return !(lhs == rhs); + } + std::vector args; }; -struct Memb { - Memb(const std::string& name) : name{name} {} +struct Memb : public Func { + explicit Memb(const std::string& name, const std::string& cls, + const std::string& ret_type, std::vector&& args, + std::vector&& args_type, + std::vector&& args_names, bool is_const, + bool is_virtual, bool is_pure, bool is_final, bool is_public) + : Func{name, ret_type, std::move(args), std::move(args_type), + std::move(args_names)}, + cls{cls}, + is_const{is_const}, + is_virtual{is_virtual}, + is_pure{is_pure}, + is_final{is_final}, + is_public{is_public} {} - std::string name; -}; + Memb clone(const std::string& new_cls) const { + auto new_args = args; + auto new_args_type = args_type; + auto new_args_names = args_names; + return Memb{name, + new_cls, + ret_type, + std::move(new_args), + std::move(new_args_type), + std::move(new_args_names), + is_const, + is_virtual, + is_pure, + is_final, + is_public}; + } -struct VMemb { - VMemb(const std::string& name, const std::string& ret_type, - const std::string cls, const std::vector&& args, - const std::vector&& args_names) - : name{name}, - ret_type{ret_type}, - cls{cls}, - args{std::move(args)}, - args_names{std::move(args_names)} {} + friend bool operator==(const Memb& lhs, const Memb& rhs) { + return lhs.is_const == rhs.is_const && + (dynamic_cast(lhs) == dynamic_cast(rhs)); + } + friend bool operator!=(const Memb& lhs, const Memb& rhs) { + return !(lhs == rhs); + } - std::string name; - std::string ret_type; - std::string cls; - std::vector const args; - std::vector const args_names; + const std::string cls; + const bool is_const; + bool is_virtual; + const bool is_pure; + const bool is_final; + const bool is_public; }; +struct Header; + struct Cls { - Cls(const std::string& name, const std::string& mother) - : name{name}, mother{mother} {} - - std::string const name; - std::string const mother; - std::vector attrs; - std::vector membs; - std::vector ctors; - std::vector vmembs; -}; + Cls(const Header& header, const std::string& name, std::string mother_name, + std::shared_ptr mother = nullptr) + : header{header}, + name{name}, + tramp_name("Py" + name), + pub_name("Pub" + name), + mother_name{mother_name}, + mother_just_name{just_name(mother_name)}, + mother{std::move(mother)}, + init{mother_name.empty()} {} -struct Enum { - Enum(const std::string& name) : name{name} {}; + auto find_overloads(const std::string& name) { + std::vector> found; + for (auto it = membs.begin(); it != membs.end();) { + it = std::find_if(it, membs.end(), + [name](std::shared_ptr memb) { + return memb->name == name; + }); + if (it != membs.end()) found.emplace_back(*it++); + } + return found; + } - std::string const name; - std::vector items; -}; + std::shared_ptr find_override(const Memb& memb) { + auto it = std::find_if( + membs.begin(), membs.end(), + [memb](std::shared_ptr other) { return *other == memb; }); + return it != membs.cend() ? *it : nullptr; + } -struct Var { - Var(const std::string& name) : name{name} {}; + std::shared_ptr find_mutable_overload(const Memb& memb) const { + auto it = std::find_if(membs.cbegin(), membs.cend(), + [memb](std::shared_ptr other) { + return !other->is_const && + other->name == memb.name && + other->args_type == memb.args_type; + }); + return it != membs.cend() ? *it : nullptr; + } - std::string const name; + auto find_vmembs() const { + std::vector> found; + for (auto it = membs.cbegin(); it != membs.cend();) { + it = std::find_if(it, membs.cend(), [](std::shared_ptr memb) { + return memb->is_virtual && !memb->is_final; + }); + if (it != membs.cend()) found.emplace_back(*it++); + } + return found; + } + + auto find_pmembs() const { + std::vector> found; + for (auto it = membs.cbegin(); it != membs.cend();) { + it = std::find_if(it, membs.cend(), [](std::shared_ptr memb) { + return !memb->is_public; + }); + if (it != membs.cend()) found.emplace_back(*it++); + } + return found; + } + + auto find_pattrs() const { + std::vector> found; + for (auto it = attrs.cbegin(); it != attrs.cend();) { + it = std::find_if(it, attrs.cend(), [](std::shared_ptr attr) { + return !attr->is_public; + }); + if (it != attrs.cend()) found.emplace_back(*it++); + } + return found; + } + + const Header& header; + const std::string name; + const std::string tramp_name; + const std::string pub_name; + const std::string mother_name; + const std::string mother_just_name; + std::shared_ptr mother{}; + std::vector> attrs{}; + std::vector> membs{}; + std::vector ctors{}; + bool init; + bool using_mother_ctor{false}; + bool has_virtual{false}; + bool has_pure{false}; + bool has_protected{false}; }; -struct Func { - Func(const std::string& name) : name{name} {}; +struct Enum { + Enum(const std::string& name) : name{name} {}; - std::string const name; + const std::string name; + std::vector items{}; }; struct Header { Header(const std::string& name) : name{name}, proper_name{get_proper_name(name)} {} + std::shared_ptr find_cls(const std::string& name) { + std::string jname = just_name(name); + auto it = std::find_if( + classes.begin(), classes.end(), + [&name](std::shared_ptr cls) { return cls->name == name; }); + return it != classes.end() ? *it : nullptr; + } + + auto find_overloads(const std::string& name) { + std::vector> found; + for (auto it = funcs.begin(); it != funcs.end();) { + it = std::find_if(it, funcs.end(), + [name](std::shared_ptr func) { + return func->name == name; + }); + if (it != funcs.end()) found.emplace_back(*it++); + } + return found; + } + static std::string get_proper_name(std::string name) { std::replace(name.begin(), name.end(), '/', '_'); return name; } - std::string const name; - std::string const proper_name; - std::vector namespaces; - std::vector classes; - std::vector enums; - std::vector vars; - std::vector funcs; + const std::string name; + const std::string proper_name; + std::vector namespaces{}; + std::vector> classes{}; + std::vector enums{}; + std::vector vars{}; + std::vector> funcs{}; + std::vector required; }; struct Module { @@ -99,9 +256,9 @@ struct Module { src{src_dir / (name + "_py.cpp")}, name{name} {} - fs::path const inc_dir; - fs::path const src_dir; - fs::path const src; + const fs::path inc_dir; + const fs::path src_dir; + const fs::path src; std::string name; - std::vector
headers; + std::vector> headers{}; }; diff --git a/src/ros2_control_py_builder/utils.hpp b/src/ros2_control_py_builder/utils.hpp index e6732f2..9def775 100644 --- a/src/ros2_control_py_builder/utils.hpp +++ b/src/ros2_control_py_builder/utils.hpp @@ -3,11 +3,14 @@ // STL #include #include +#include #include #include -#include +#include +#include #include #include +#include // CppParser #include #include @@ -46,6 +49,8 @@ std::ostream& operator<<(std::ostream& os, const Sep& sep); inline std::string str_of_cpp(CppWriter& writer, const CppObj* cppObj); /// @brief std::string to upper (not in place) inline std::string to_upper(std::string str); +inline std::string just_name(const std::string& name); +inline std::string just_name(std::string&& name); // Impl @@ -133,3 +138,142 @@ inline std::string to_upper(std::string str) { [](char c) { return std::toupper(c); }); return str; } + +inline std::string just_name(const std::string& name) { + return just_name(std::string{name}); +} + +inline std::string just_name(std::string&& name) { + auto it = std::search_n(name.crbegin(), name.crend(), 2, ':'); + if (it != name.crend()) name.erase(name.cbegin(), it.base()); + return std::string{std::move(name)}; +} + +template +class PtrIt { + public: + explicit PtrIt(T it) : it_{it} {} + + decltype(std::declval()->operator*()) operator*() const { + return it_->operator*(); + } + auto operator->() const { return std::addressof(this->operator*()); } + + PtrIt operator++() { return PtrIt{++it_}; } + PtrIt operator++(int) { return PtrIt{it_++}; } + + PtrIt operator--() { return PtrIt{--it_}; } + PtrIt operator--(int) { return PtrIt{it_--}; } + + PtrIt operator+(std::ptrdiff_t i) const { return PtrIt{it_} += i; } + PtrIt& operator+=(std::ptrdiff_t i) { + it_ += i; + return *this; + } + + PtrIt operator-(std::ptrdiff_t i) const { return PtrIt{it_} -= i; } + PtrIt& operator-=(std::ptrdiff_t i) { + it_ -= i; + return *this; + } + + bool operator==(const PtrIt& other) const { return it_ == other.it_; } + bool operator!=(const PtrIt& other) const { return !(*this == other); } + + T base() const { return it_; } + void from_base(T it) { it_ = it; } + + private: + T it_; +}; + +template +class CPtrIterable { + public: + explicit CPtrIterable(const T& iterable) noexcept : iterable_{iterable} {} + + auto begin() const { return PtrIt{std::begin(iterable_)}; } + auto end() const { return PtrIt{std::end(iterable_)}; } + + auto cbegin() const { return PtrIt{std::cbegin(iterable_)}; } + auto cend() const { return PtrIt{std::cend(iterable_)}; } + + auto rbegin() const { return PtrIt{std::rbegin(iterable_)}; } + auto rend() const { return PtrIt{std::rend(iterable_)}; } + + auto crbegin() const { return PtrIt{std::crbegin(iterable_)}; } + auto crend() const { return PtrIt{std::crend(iterable_)}; } + + private: + const T& iterable_; +}; + +template +class PtrIterable { + public: + explicit PtrIterable(T& iterable) noexcept : iterable_{iterable} {} + + auto begin() { return PtrIt{std::begin(iterable_)}; } + auto end() { return PtrIt{std::end(iterable_)}; } + + auto begin() const { return PtrIt{std::begin(iterable_)}; } + auto end() const { return PtrIt{std::end(iterable_)}; } + + auto cbegin() const { return PtrIt{std::cbegin(iterable_)}; } + auto cend() const { return PtrIt{std::cend(iterable_)}; } + + auto rbegin() { return PtrIt{std::rbegin(iterable_)}; } + auto rend() { return PtrIt{std::rend(iterable_)}; } + + auto rbegin() const { return PtrIt{std::rbegin(iterable_)}; } + auto rend() const { return PtrIt{std::rend(iterable_)}; } + + auto crbegin() const { return PtrIt{std::crbegin(iterable_)}; } + auto crend() const { return PtrIt{std::crend(iterable_)}; } + + private: + T& iterable_; +}; + +template +class OwningPtrIterable { + public: + explicit OwningPtrIterable(T&& iterable) noexcept + : iterable_{std::move(iterable)} {} + + auto begin() { return PtrIt{std::begin(iterable_)}; } + auto end() { return PtrIt{std::end(iterable_)}; } + + auto begin() const { return PtrIt{std::begin(iterable_)}; } + auto end() const { return PtrIt{std::end(iterable_)}; } + + auto cbegin() const { return PtrIt{std::cbegin(iterable_)}; } + auto cend() const { return PtrIt{std::cend(iterable_)}; } + + auto rbegin() { return PtrIt{std::rbegin(iterable_)}; } + auto rend() { return PtrIt{std::rend(iterable_)}; } + + auto rbegin() const { return PtrIt{std::rbegin(iterable_)}; } + auto rend() const { return PtrIt{std::rend(iterable_)}; } + + auto crbegin() const { return PtrIt{std::crbegin(iterable_)}; } + auto crend() const { return PtrIt{std::crend(iterable_)}; } + + private: + T iterable_; +}; + +template +auto ptr_iter(T&& iterable) { + return OwningPtrIterable{std::move(iterable)}; +} + +template +auto ptr_iter(T& iterable) { + return PtrIterable{iterable}; +} + +template +auto ptr_iter(const T& iterable) { + return CPtrIterable{iterable}; +} diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index 55a98b6..482fd8a 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -1,6 +1,7 @@ #pragma once // STL +#include #include // boost #include @@ -12,8 +13,10 @@ namespace fs = boost::filesystem; /// @brief output a `Cls` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Cls& cls); -/// @brief output a `VMemb` to write an hpp -inline std::ostream& operator<<(std::ostream& os, const VMemb& vmemb); +/// @brief output a `Memb` to write an hpp +inline std::ostream& operator<<(std::ostream& os, const Memb& memb); +/// @brief output a `Memb` to write an hpp +inline std::ostream& operator<<(std::ostream& os, const Memb& vmemb); /// @brief output a `Enum` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Enum& enu); /// @brief output a `Var` to write an hpp @@ -28,44 +31,49 @@ inline void write_module(const Module& mod); // Impl inline std::ostream& operator<<(std::ostream& os, const Cls& cls) { - if (!cls.vmembs.empty()) { - Cls py_cls{cls.name, "Py" + cls.name}; - py_cls.attrs = cls.attrs; - py_cls.ctors = cls.ctors; - py_cls.membs = cls.membs; - py_cls.membs.reserve(py_cls.membs.size() + cls.vmembs.size()); - for (const VMemb& vmemb : cls.vmembs) py_cls.membs.emplace_back(vmemb.name); - return os << py_cls; - } - os << " py::class_<" << cls.name - << (cls.mother.empty() ? "" : ", " + cls.mother) << ">(m, \"" << cls.name - << "\")"; + os << " py::class_<" << cls.name; + if (cls.mother) os << ", " << cls.mother->name; + if (cls.has_virtual) os << ", " << cls.tramp_name; + os << ">(m, \"" << cls.name << "\")"; std::vector ctors = cls.ctors; if (ctors.empty()) ctors.emplace_back(std::vector{}); for (const Ctor& ctor : ctors) - os << "\n .def(py::init<" << Sep(ctor.args, ", ") << ">())"; - for (const Memb& memb : cls.membs) - os << "\n .def(\"" << memb.name << "\", &" << cls.name - << "::" << memb.name << ")"; - for (const Attr& attr : cls.attrs) - os << "\n .def_readwrite(\"" << attr.name << "\", &" << cls.name - << "::" << attr.name << ")"; + os << "\n .def(py::init<" << Sep{ctor.args, ", "} << ">())"; + for (const Memb& memb : ptr_iter(cls.membs)) { + if (memb.is_overloaded) + os << "\n .def(\"" << memb.name << "\", [](" + << (memb.is_public ? cls.name : cls.pub_name) << "& py_self" + << (memb.args.empty() ? "" : ", ") << Sep{memb.args, ", "} + << ") { return py_self." << memb.name << '(' + << Sep{memb.args_names, ", "} << "); })"; + else + os << "\n .def(\"" << memb.name << "\", &" + << (memb.is_public ? cls.name : cls.pub_name) << "::" << memb.name + << ")"; + } + for (const Attr& attr : ptr_iter(cls.attrs)) + os << "\n .def_readwrite(\"" << attr.name << "\", &" + << (attr.is_public ? cls.name : cls.pub_name) << "::" << attr.name + << ")"; os << ";\n"; return os; } -inline std::ostream& operator<<(std::ostream& os, const VMemb& vmemb) { - os << "\n " << vmemb.ret_type << ' ' << vmemb.name << '(' - << Sep(vmemb.args, ", ") << R"() override { - PYBIND11_OVERRIDE_PURE( - )" - << vmemb.ret_type << R"(, - )" - << vmemb.cls << R"(, - )" - << vmemb.name; - for (const std::string& arg : vmemb.args_names) os << ",\n " << arg; - return os << R"( +inline std::ostream& operator<<(std::ostream& os, const Memb& memb) { + ASSERT(memb.is_virtual && !memb.is_final, + memb.cls << "::" << memb.name + << " is not a virtual member but it was used as one"); + return os << "\n " << memb.ret_type << ' ' << memb.name << '(' + << Sep{memb.args, ", "} << ") " << (memb.is_const ? "const " : "") + << R"(override { + PYBIND11_OVERRIDE)" + << (memb.is_pure ? "_PURE" : "") << R"(( + )" << memb.ret_type + << R"(, + )" << memb.cls + << R"(, + )" << memb.name + << ",\n " << Sep{memb.args_names, ",\n "} << R"( ); } )"; @@ -81,19 +89,27 @@ inline std::ostream& operator<<(std::ostream& os, const Enum& enu) { } inline std::ostream& operator<<(std::ostream& os, const Var& var) { - return os << " m.def(\"" << var.name << "\", []() { return std::string{" - << var.name << "}; });\n"; + return os << " m.def(\"" << var.name << "\", []() { return " << var.name + << "; });\n"; } inline std::ostream& operator<<(std::ostream& os, const Func& func) { + if (func.is_overloaded) + return os << "\n .def(\"" << func.name << "\", [](" + << Sep{func.args, ", "} << ") { return " << func.name << '(' + << Sep{func.args_names, ", "} << "); })"; return os << " m.def(\"" << func.name << "\", &" << func.name << ");\n"; } void write_module_header(const Module& mod, const Header& header) { fs::path path = mod.src_dir / (header.name + "_py.hpp"); + fs::create_directories(path.parent_path()); + ASSERT_DIR(path.parent_path()); std::ofstream ofs{path, std::ios::out | std::ios::trunc}; ASSERT(ofs, "Could not open " << path); - ofs << R"(// pybind11 + ofs << R"(#pragma once + +// pybind11 #include // )" << mod.name << R"( @@ -108,27 +124,38 @@ namespace py = pybind11; )"; for (const std::string& ns : header.namespaces) ofs << "using namespace " << ns << ";\n"; - for (const Cls& cls : header.classes) { - if (cls.vmembs.empty()) continue; - ofs << "\nclass Py" << cls.name << ": public " << cls.name + for (const Cls& cls : ptr_iter(header.classes)) { + if (!cls.has_virtual) continue; + ofs << "\nclass " << cls.tramp_name << ": public " << cls.name << " {\n public:\n using " << cls.name << "::" << cls.name << ";\n"; - for (const VMemb& vmemb : cls.vmembs) ofs << vmemb; + for (const Memb& vmemb : ptr_iter(cls.find_vmembs())) ofs << vmemb; + ofs << "};\n"; + } + for (const Cls& cls : ptr_iter(header.classes)) { + if (!cls.has_protected) continue; + ofs << "\nclass " << cls.pub_name << ": public " << cls.name + << " {\n public:\n"; + for (const Memb& pmemb : ptr_iter(cls.find_pmembs())) + ofs << " using " << cls.name << "::" << pmemb.name << ";\n"; + for (const Attr& pattr : ptr_iter(cls.find_pattrs())) + ofs << " using " << cls.name << "::" << pattr.name << ";\n"; ofs << "};\n"; } ofs << R"( inline void init_)" << header.proper_name << R"((py::module &m) { -)" << Sep(header.vars, "\n") - << Sep(header.funcs, "\n") << Sep(header.enums, "\n") - << Sep(header.classes, "\n") << R"(} +)" << Sep{header.vars, "\n"} + << Sep{ptr_iter(header.funcs), "\n"} << Sep{header.enums, "\n"} + << Sep{ptr_iter(header.classes), "\n"} << R"(} } )"; } void write_module(const Module& mod) { - for (const Header& header : mod.headers) write_module_header(mod, header); + for (const Header& header : ptr_iter(mod.headers)) + write_module_header(mod, header); std::ofstream ofs{mod.src, std::ios::out | std::ios::trunc}; ASSERT(ofs, "could not open " << mod.src); @@ -137,13 +164,14 @@ void write_module(const Module& mod) { // )" << mod.name << '\n'; - for (const Header& header : mod.headers) + for (const Header& header : ptr_iter(mod.headers)) ofs << "#include \"" << mod.name << "/" << header.name << "_py.hpp\"\n"; ofs << R"( namespace py = pybind11; PYBIND11_MODULE()" - << mod.name << R"(, m) + << mod.name + "_py" + << R"(, m) { m.doc() = R"doc( Python bindings for ros2_control functionalities. @@ -155,7 +183,7 @@ PYBIND11_MODULE()" // Construct module classes )"; - for (const Header& header : mod.headers) { + for (const Header& header : ptr_iter(mod.headers)) { ofs << " ros2_control_py::bind_" << mod.name << "::init_" << header.proper_name << "(m);\n"; } From 807be8972d4e7309e3d0a70c502a057dbc67dfdd Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 11 Oct 2023 09:04:46 +0200 Subject: [PATCH 21/42] fix: removed unneeded code --- src/ros2_control_py_builder/write.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index 482fd8a..060284b 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -1,7 +1,6 @@ #pragma once // STL -#include #include // boost #include @@ -15,8 +14,6 @@ namespace fs = boost::filesystem; inline std::ostream& operator<<(std::ostream& os, const Cls& cls); /// @brief output a `Memb` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Memb& memb); -/// @brief output a `Memb` to write an hpp -inline std::ostream& operator<<(std::ostream& os, const Memb& vmemb); /// @brief output a `Enum` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Enum& enu); /// @brief output a `Var` to write an hpp From 9c49ee276fd232d36a3584e1e09a0f7ec9ece459 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 11 Oct 2023 09:11:05 +0200 Subject: [PATCH 22/42] feat: updated readme testing example --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 4ae6a5b..9293985 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,6 @@ colcon build --packages-select ros2_control_py To create an actuator ```python -from ros2_control_py import Actuator -actuator = Actuator() +from ros2_control_py import hardware_interface +actuator = hardware_interface.Actuator() ``` From b3beff498bfeef0b012892ffcbf2a0f6cdcbf17d Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 18 Oct 2023 11:32:40 +0200 Subject: [PATCH 23/42] feat: passing tests on humble --- .gitignore | 7 + CMakeLists.txt | 71 +- README.md | 17 +- doc/index.rst | 6 + package.xml | 8 +- python/ros2_control_py/__init__.py | 17 + ros2_control_py/__init__.py | 13 - src/ros2_control_py_builder/main.cpp | 167 ++-- src/ros2_control_py_builder/parse.hpp | 157 +++- src/ros2_control_py_builder/structs.hpp | 20 +- src/ros2_control_py_builder/utils.hpp | 139 +++- src/ros2_control_py_builder/write.hpp | 277 ++++++- tests/__init__.py | 0 tests/hardware_interface/__init__.py | 0 .../test_component_interface.py | 201 +++++ .../test_component_parser.py | 715 ++++++++++++++++++ tests/hardware_interface/test_handle.py | 47 ++ .../hardware_interface/test_inst_hardware.py | 13 + tests/test_sanity.py | 5 + tests/utils.py | 11 + 20 files changed, 1709 insertions(+), 182 deletions(-) create mode 100644 .gitignore create mode 100644 doc/index.rst create mode 100644 python/ros2_control_py/__init__.py delete mode 100644 ros2_control_py/__init__.py create mode 100644 tests/__init__.py create mode 100644 tests/hardware_interface/__init__.py create mode 100644 tests/hardware_interface/test_component_interface.py create mode 100644 tests/hardware_interface/test_component_parser.py create mode 100644 tests/hardware_interface/test_handle.py create mode 100644 tests/hardware_interface/test_inst_hardware.py create mode 100644 tests/test_sanity.py create mode 100644 tests/utils.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..372a845 --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +build +.cache +compile_commands.json +tmp +install +log +__pycache__ diff --git a/CMakeLists.txt b/CMakeLists.txt index cd169d7..ccc943a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.20) -project(ros2_control_py) +project(ros2_control_py VERSION 0.0.2) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -12,10 +12,11 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 11) endif() if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra) + add_compile_options(-Wall -Wextra -Wpedantic) endif() find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) find_package(rclpy REQUIRED) find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) @@ -23,44 +24,68 @@ find_package(pybind11 REQUIRED) add_subdirectory(cppparser) -set(ros2_py_src_dir ${CMAKE_CURRENT_BINARY_DIR}/src) +set(_ros2_py_src_dir ${CMAKE_CURRENT_BINARY_DIR}/src) -set(ros2_py_modules +set(_ros2_py_modules hardware_interface controller_interface + ros2_control_test_assets ) -set(ros2_py_src ${ros2_py_modules}) -list(TRANSFORM ros2_py_src PREPEND ${ros2_py_src_dir}/) -list(TRANSFORM ros2_py_src APPEND _py.cpp) +set(_ros2_py_src ${_ros2_py_modules}) +list(TRANSFORM _ros2_py_src PREPEND ${_ros2_py_src_dir}/) +list(TRANSFORM _ros2_py_src APPEND _py.cpp) -add_executable(ros2_control_py_builder - src/ros2_control_py_builder/main.cpp +add_executable(${PROJECT_NAME}_builder + src/${PROJECT_NAME}_builder/main.cpp ) -target_link_libraries(ros2_control_py_builder +target_link_libraries(${PROJECT_NAME}_builder PRIVATE cppparser ) add_custom_command( - OUTPUT ${ros2_py_src} - COMMAND ros2_control_py_builder "${CMAKE_CURRENT_BINARY_DIR}" "/opt/ros/$ENV{ROS_DISTRO}/include" ${ros2_py_modules} - DEPENDS ros2_control_py_builder + OUTPUT ${_ros2_py_src} + COMMAND ${PROJECT_NAME}_builder "${CMAKE_CURRENT_BINARY_DIR}" "/opt/ros/$ENV{ROS_DISTRO}/include" ${_ros2_py_modules} + DEPENDS ${PROJECT_NAME}_builder ) -ament_python_install_package(ros2_control_py) - -foreach(module ${ros2_py_modules}) - find_package(${module} REQUIRED) - pybind11_add_module(${module}_py - ${ros2_py_src_dir}/${module}_py.cpp +configure_file("${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}/__init__.py" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}/__init__.py" + COPYONLY +) +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR "${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}" +) +foreach(_module ${_ros2_py_modules}) + find_package(${_module} REQUIRED) + pybind11_add_module(${_module}_py + "${_ros2_py_src_dir}/${_module}_py.cpp" + ) + set_target_properties(${_module}_py + PROPERTIES OUTPUT_NAME ${PROJECT_NAME}/${_module} ) - ament_target_dependencies(${module}_py PUBLIC + target_include_directories(${_module}_py PRIVATE + "${_ros2_py_src_dir}" + ) + ament_target_dependencies(${_module}_py PUBLIC rclpy - ${module} + ${_module} ) - install(TARGETS ${module}_py + install(TARGETS ${_module}_py DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" ) endforeach() +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + file(GLOB_RECURSE _pytest_tests CONFIGURE_DEPENDS RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "tests/test_*.py" "tests/*/test_*.py") + foreach(_test_path ${_pytest_tests}) + cmake_path(RELATIVE_PATH _test_path + BASE_DIRECTORY "tests/" + OUTPUT_VARIABLE _test_name + ) + ament_add_pytest_test(${_test_name} ${_test_path}) + endforeach() +endif() + ament_package() diff --git a/README.md b/README.md index 9293985..8dfe5bf 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ ## Install -Assuming that your [ros2_control](https://github.com/ros-controls/ros2_control) workspace is ìn the directory `~/ros2_control_py_ws`: +Assuming that your [ros2_control](https://github.com/ros-controls/ros2_control) workspace is in the directory `~/ros2_control_py_ws`: ```sh mkdir -p ~/ros2_control_py_ws/src @@ -18,14 +18,17 @@ rosdep install --from-paths src -y --ignore-src ```sh cd ~/ros2_control_py_ws -colcon build --packages-select ros2_control_py +colcon build ``` ## Testing -To create an actuator - -```python -from ros2_control_py import hardware_interface -actuator = hardware_interface.Actuator() +```sh +cd ~/ros2_control_py_ws +colcon build +colcon test ``` + +## Usage + +See [doc](doc/index.rst) and [tests](tests/). diff --git a/doc/index.rst b/doc/index.rst new file mode 100644 index 0000000..9c00d6c --- /dev/null +++ b/doc/index.rst @@ -0,0 +1,6 @@ + +=============== +ros2_control_py +=============== + +Nothing here for now... diff --git a/package.xml b/package.xml index 053e373..887108e 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ ros2_control_py - 0.0.1 + 0.0.2 Python binding for ros2_control Olivier Stasse @@ -10,15 +10,16 @@ Apache License 2.0 Olivier Stasse + Maxence MICHOT ament_cmake + ament_cmake_python pybind11_vendor + flex ament_index_python - flex - rclcpp rclcpp_lifecycle rclpy @@ -31,7 +32,6 @@ transmission_interface ament_cmake_pytest - python3-pytest ament_cmake diff --git a/python/ros2_control_py/__init__.py b/python/ros2_control_py/__init__.py new file mode 100644 index 0000000..bd4df8c --- /dev/null +++ b/python/ros2_control_py/__init__.py @@ -0,0 +1,17 @@ +import pkgutil +import importlib + +loader = None +module_name = None +is_pkg = None + +__all__ = [] +for loader, module_name, is_pkg in pkgutil.walk_packages(__path__): + __all__.append(module_name) + globals()[module_name] = importlib.import_module(f"ros2_control_py.{module_name}") + +del pkgutil +del importlib +del loader +del module_name +del is_pkg diff --git a/ros2_control_py/__init__.py b/ros2_control_py/__init__.py deleted file mode 100644 index 5ffeae5..0000000 --- a/ros2_control_py/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -import pkgutil - -__all__ = [] -for loader, module_name, is_pkg in pkgutil.walk_packages(__path__): - __all__.append(module_name[:-3]) - globals()[module_name[:-3]] = loader.find_module(module_name).load_module( - module_name - ) - -del pkgutil -del loader -del module_name -del is_pkg diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index c9c37c2..1c4624b 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -1,10 +1,116 @@ // STL +#include #include // ros2_control_py_builder #include "parse.hpp" #include "utils.hpp" #include "write.hpp" +/// @brief init py_utils header for a module +void init_py_utils(Module& mod); +/// @brief find each classes' mother +void init_cls_mothers(Module& mod); +void init_cls_vmembs(Module& mod); +void init_overloads(Module& mod); +void init_header_order(Module& mod); + +int main(int argc, char** argv) { + ASSERT(argc > 3, + "Invalid number of command line arguments, expected at least 3 got " + << argc - 1); + + fs::path dst_dir = argv[1]; + fs::path inc_dir = argv[2]; + fs::path src_dir = dst_dir / "src"; + fs::create_directories(src_dir); + + ASSERT_DIR(src_dir); + ASSERT_DIR(inc_dir); + + std::vector modules; + for (int i = 3; i < argc; ++i) + modules.emplace_back(inc_dir, src_dir, argv[i]); + + for (const Module& mod : modules) { + fs::create_directories(mod.src_dir); + ASSERT_DIR(mod.src_dir); + ASSERT_DIR(mod.inc_dir); + } + + for (Module& mod : modules) { + for (const fs::directory_entry& entry : + fs::recursive_directory_iterator{mod.inc_dir}) { + const fs::path& path = entry.path(); + if (!fs::is_regular_file(entry) || path.extension() != ".hpp" || + path.filename() == "macros.hpp") + continue; + + std::string name = path.lexically_relative(mod.inc_dir).string(); + parse_header(mod, path, name); + } + + init_py_utils(mod); + init_cls_mothers(mod); + init_cls_vmembs(mod); + init_overloads(mod); + init_header_order(mod); + } + + for (const Module& mod : modules) write_module(mod); + + return 0; +} + +void init_py_utils(Module& mod) { + for (Header& header : ptr_iter(mod.headers)) { + header.required.emplace_back(mod.py_utils->name); + auto set_remove = [](Header& header, const std::string& name) { + auto it = std::find_if(header.stl_bind.cbegin(), header.stl_bind.cend(), + [name](const auto& bind) { + return std::get<2>(bind) == name || + std::get<3>(bind) == name; + }); + if (it != header.stl_bind.cend()) header.stl_bind.erase(it); + }; + set_remove(header, "hardware_interface::CommandInterface"); + set_remove(header, "hardware_interface::StateInterface"); + set_remove(header, "CommandInterface"); + set_remove(header, "StateInterface"); + set_remove(header, "hardware_interface::LoanedCommandInterface"); + set_remove(header, "hardware_interface::LoanedStateInterface"); + set_remove(header, "LoanedCommandInterface"); + set_remove(header, "LoanedStateInterface"); + mod.py_utils->stl_bind.merge(header.stl_bind); + header.stl_bind.clear(); + for (const std::string& ns : header.namespaces) + mod.py_utils->namespaces.insert(ns); + } + mod.headers.emplace_back(mod.py_utils); + + if (mod.name != "hardware_interface") return; + auto lni = + std::make_shared(*mod.py_utils, "rclcpp_lifecycle::node_interfaces", + "LifecycleNodeInterface", "", nullptr); + auto names = {"on_configure", "on_cleanup", "on_shutdown", + "on_activate", "on_deactivate", "on_error"}; + for (const auto& name : names) + lni->membs.emplace_back( + new Memb{name, + lni->complete_name, + "CallbackReturn", + {"const rclcpp_lifecycle::State& previous_state"}, + {"const rclcpp_lifecycle::State&"}, + {"previous_state"}, + false, + true, + false, + false, + true}); + lni->has_virtual = true; + lni->is_outsider = true; + mod.py_utils->classes.emplace_back(lni); +} + void init_cls_mothers(Module& mod) { std::size_t size = 0; for (Header& header : ptr_iter(mod.headers)) size += header.classes.size(); @@ -21,19 +127,14 @@ void init_cls_mothers(Module& mod) { if (cls.init) continue; auto it = classes.find(cls.mother_just_name); if (it == classes.end()) { - std::cerr << cls.name << " did not find " << cls.mother_just_name - << std::endl; + std::cerr << "warning: class " << cls.name << " did not find mother " + << cls.mother_just_name << std::endl; cls.init = true; continue; } cls.mother = it->second; if (header.name != cls.mother->header.name) header.required.emplace_back(cls.mother->header.name); - if (!cls.using_mother_ctor) continue; - for (const Ctor& ctor : cls.mother->ctors) - if (std::find(cls.ctors.cbegin(), cls.ctors.cend(), ctor) == - cls.ctors.cend()) - cls.ctors.emplace_back(ctor); } } } @@ -50,6 +151,12 @@ void init_cls_vmembs(Module& mod) { continue; } cls.init = true; + if (cls.using_mother_ctor) { + for (const Ctor& ctor : cls.mother->ctors) + if (std::find(cls.ctors.cbegin(), cls.ctors.cend(), ctor) == + cls.ctors.cend()) + cls.ctors.emplace_back(ctor); + } if (!cls.mother->has_virtual) continue; for (Memb const& vmemb : ptr_iter(cls.mother->find_vmembs())) { auto ovrd = cls.find_override(vmemb); @@ -122,49 +229,3 @@ void init_header_order(Module& mod) { } mod.headers = std::move(headers); } - -int main(int argc, char** argv) { - ASSERT(argc > 3, - "Invalid number of command line arguments, expected at least 3 got " - << argc - 1); - - fs::path dst_dir = argv[1]; - fs::path inc_dir = argv[2]; - fs::path src_dir = dst_dir / "src"; - fs::create_directories(src_dir); - - ASSERT_DIR(src_dir); - ASSERT_DIR(inc_dir); - - std::vector modules; - for (int i = 3; i < argc; ++i) - modules.emplace_back(inc_dir, src_dir, argv[i]); - - for (const Module& mod : modules) { - fs::create_directories(mod.src_dir); - ASSERT_DIR(mod.src_dir); - ASSERT_DIR(mod.inc_dir); - } - - for (Module& mod : modules) { - for (const fs::directory_entry& entry : - fs::recursive_directory_iterator{mod.inc_dir}) { - const fs::path& path = entry.path(); - if (!fs::is_regular_file(entry) || path.extension() != ".hpp" || - path.filename() == "macros.hpp") - continue; - - std::string name = path.lexically_relative(mod.inc_dir).string(); - parse_header(mod, path, name); - } - - init_cls_mothers(mod); - init_cls_vmembs(mod); - init_overloads(mod); - init_header_order(mod); - } - - for (const Module& mod : modules) write_module(mod); - - return 0; -} diff --git a/src/ros2_control_py_builder/parse.hpp b/src/ros2_control_py_builder/parse.hpp index 6ce60e2..649a2a8 100644 --- a/src/ros2_control_py_builder/parse.hpp +++ b/src/ros2_control_py_builder/parse.hpp @@ -16,64 +16,117 @@ namespace fs = boost::filesystem; +inline void find_stl(Header& header, const std::string& type_name, + const std::string& type, const std::string& cpp_type) { + std::string beg_str = "std::" + cpp_type + "<"; + std::size_t beg = type_name.find(beg_str); + if (beg == std::string::npos) return; + beg += beg_str.size(); + std::string u = type_name.substr(beg, type_name.rfind('>') - beg); + std::string v = ""; + if (type == "map") { + std::size_t i = u.find(','); + v = u.substr(i + 1); + if (!v.empty() && v[0] == ' ') v.erase(v.begin()); + u.erase(u.begin() + i, u.end()); + } + header.stl_bind.insert({type, cpp_type, u, v}); +} + +inline void find_stls(Header& header, const std::string& type_name) { + find_stl(header, type_name, "vector", "vector"); + find_stl(header, type_name, "map", "unordered_map"); + find_stl(header, type_name, "map", "unordered_multimap"); + find_stl(header, type_name, "map", "map"); + find_stl(header, type_name, "map", "multimap"); +} + inline void parse_class_attr(Cls& cls, CppConstVarEPtr attr) { + if (attr->templateParamList()) { + std::cerr << "warning: class " << cls.name << " skipped template attr " + << attr->name() << std::endl; + return; + } if (attr->varType()->typeModifier().refType_ != CppRefType::kNoRef) { - std::cerr << "skipped ref attr " << cls.name << "::" << attr->name() - << std::endl; + std::cerr << "warning: class " << cls.name << " skipped ref attr " + << attr->name() << std::endl; + return; + } + std::string const type_name = str_of_cpp(attr->varType()); + if (type_name.find("std::vector<") != std::string::npos && + (type_name.find("CommandInterface") != std::string::npos || + type_name.find("StateInterface") != std::string::npos)) { + std::cerr << "warning: class " << cls.name << " skipped vector attr " + << attr->name() << std::endl; return; } - if (!isPublic(attr)) return; + find_stls(cls.header, type_name); cls.attrs.emplace_back(std::make_shared(attr->name(), isPublic(attr))); if (!cls.attrs.back()->is_public) cls.has_protected = true; } -inline void parse_class_ctor(Cls& cls, CppConstructorEPtr ctor, - CppWriter& writer) { +inline void parse_class_ctor(Cls& cls, CppConstructorEPtr ctor) { + if (ctor->templateParamList()) { + std::cerr << "warning: class " << cls.name << " skipped template ctor" + << std::endl; + return; + } std::vector args; const CppParamVector* params = ctor->params(); bool valid = true; + std::size_t defaulted = 0; if (params) { for (const CppObjPtr& param : *params) { CppConstVarEPtr var = param; ASSERT(var, "that was not a var"); - std::string arg = str_of_cpp(writer, var->varType()); - if (arg == "Deleter&&" || - arg.find("std::unique_ptr<") != std::string::npos) { + if (var->assignValue()) ++defaulted; + std::string const arg = str_of_cpp(var->varType()); + find_stls(cls.header, arg); + if (arg == "Deleter&&") { valid = false; break; } args.emplace_back(std::move(arg)); } } - if (valid) cls.ctors.emplace_back(std::move(args)); + if (!valid) return; + for (; defaulted > 0; --defaulted) { + std::vector args_cpy{args.cbegin(), args.cend() - defaulted}; + cls.ctors.emplace_back(std::move(args_cpy)); + } + cls.ctors.emplace_back(std::move(args)); } -inline void parse_class_memb(Cls& cls, CppConstFunctionEPtr memb, - CppWriter& writer) { +inline void parse_class_memb(Cls& cls, CppConstFunctionEPtr memb) { if (memb->templateParamList()) { - std::cerr << cls.name << ": skipped template member " << memb->name_ - << std::endl; + std::cerr << "warning: class " << cls.name << " skipped template member " + << memb->name_ << std::endl; return; } - std::string ret_type = str_of_cpp(writer, memb->retType_.get()); + std::string const ret_type = str_of_cpp(memb->retType_.get()); + find_stls(cls.header, ret_type); std::vector args; std::vector args_type; std::vector args_names; + std::size_t defaulted = 0; const CppParamVector* params = memb->params(); if (params) { - size_t i = 0; + std::size_t i = 0; for (const CppObjPtr& param : *params) { CppConstVarEPtr var = param; ASSERT(var, "that was not a var"); + if (var->assignValue()) ++defaulted; /*if (var->varType()->typeModifier().refType_ == CppRefType::kByRef) std::cerr << "warning: arg by ref in " << cls.name << "::" << memb->name_ << std::endl;*/ if (var->varType()->typeModifier().refType_ == CppRefType::kRValRef) { - std::cerr << "warning: skipped " << cls.name << "::" << memb->name_ - << " because of rvalue argument" << std::endl; + std::cerr << "warning: class " << cls.name + << " skipped memb with rvalue arg " << memb->name_ + << std::endl; return; } - std::string type = str_of_cpp(writer, var->varType()); + std::string type = str_of_cpp(var->varType()); + find_stls(cls.header, type); std::string name = !var->name().empty() ? var->name() : "arg" + std::to_string(i++); args.emplace_back(type + " " + name); @@ -81,6 +134,18 @@ inline void parse_class_memb(Cls& cls, CppConstFunctionEPtr memb, args_names.emplace_back(std::move(name)); } } + for (; defaulted > 0; --defaulted) { + std::vector args_cpy{args.cbegin(), args.cend() - defaulted}; + std::vector args_type_cpy{args_type.cbegin(), + args_type.cend() - defaulted}; + std::vector args_names_cpy{args_names.cbegin(), + args_names.cend() - defaulted}; + cls.membs.emplace_back(std::make_shared( + memb->name_, cls.name, ret_type, std::move(args_cpy), + std::move(args_type_cpy), std::move(args_names_cpy), + isConst(memb.get()), isVirtual(memb.get()), isPureVirtual(memb.get()), + isFinal(memb.get()), isPublic(memb.get()))); + } cls.membs.emplace_back(std::make_shared( memb->name_, cls.name, ret_type, std::move(args), std::move(args_type), std::move(args_names), isConst(memb.get()), isVirtual(memb.get()), @@ -93,15 +158,15 @@ inline void parse_class_memb(Cls& cls, CppConstFunctionEPtr memb, inline void parse_class_using(Cls& cls) { cls.using_mother_ctor = true; } -inline void parse_class(Header& header, CppWriter& writer, - CppConstCompoundEPtr cls) { +inline void parse_class(Header& header, CppConstCompoundEPtr cls, + const std::string& ns) { const CppInheritanceListPtr& parents = cls->inheritanceList(); ASSERT(!parents || parents->size() <= 1, "Too many parents for " << cls->name()); bool has_mother = parents && !parents->empty() && parents->front().inhType == CppAccessType::kPublic; auto cls_rep = std::make_shared( - header, cls->name(), has_mother ? parents->front().baseName : ""); + header, ns, cls->name(), has_mother ? parents->front().baseName : ""); header.classes.emplace_back(cls_rep); for (const CppObjPtr& obj_memb : cls->members()) { if (!isPublic(obj_memb) && !isProtected(obj_memb)) continue; @@ -113,13 +178,13 @@ inline void parse_class(Header& header, CppWriter& writer, CppConstFunctionEPtr memb = obj_memb; if (memb && memb->name_.find("operator") == std::string::npos && memb->name_ != "get_full_name" && memb->name_ != "import_component") { - parse_class_memb(*cls_rep, memb, writer); + parse_class_memb(*cls_rep, memb); continue; } if (!isPublic(obj_memb)) continue; CppConstructorEPtr ctor = obj_memb; if (ctor && !ctor->isCopyConstructor() && !ctor->isMoveConstructor()) { - parse_class_ctor(*cls_rep, ctor, writer); + parse_class_ctor(*cls_rep, ctor); continue; } CppConstUsingDeclEPtr use = obj_memb; @@ -139,31 +204,36 @@ inline void parse_enum(Header& header, CppEnumEPtr enu) { inline void parse_var(Header& header, CppVarEPtr var) { if (var->templateParamList()) { - std::cerr << header.name << ": skipped template var " << var->name() - << std::endl; + std::cerr << "warning: header " << header.name << " skipped template var " + << var->name() << std::endl; return; } + std::string const type_name = str_of_cpp(var->varType()); + find_stls(header, type_name); header.vars.emplace_back(var->name()); } -inline void parse_func(Header& header, CppWriter& writer, - CppFunctionEPtr func) { +inline void parse_func(Header& header, CppFunctionEPtr func) { if (func->templateParamList()) { - std::cerr << header.name << ": skipped template function " << func->name_ - << std::endl; + std::cerr << "warning: header " << header.name + << " skipped template function " << func->name_ << std::endl; return; } - std::string ret_type = str_of_cpp(writer, func->retType_.get()); + std::string ret_type = str_of_cpp(func->retType_.get()); + find_stls(header, ret_type); std::vector args; std::vector args_type; std::vector args_names; + std::size_t defaulted = 0; const CppParamVector* params = func->params(); if (params) { size_t i = 0; for (const CppObjPtr& param : *params) { CppConstVarEPtr var = param; ASSERT(var, "that was not a var"); - std::string type = str_of_cpp(writer, var->varType()); + if (var->assignValue()) ++defaulted; + std::string type = str_of_cpp(var->varType()); + find_stls(header, type); std::string name = !var->name().empty() ? var->name() : "arg" + std::to_string(i++); args.emplace_back(type + " " + name); @@ -171,14 +241,24 @@ inline void parse_func(Header& header, CppWriter& writer, args_names.emplace_back(std::move(name)); } } + for (; defaulted > 0; --defaulted) { + std::vector args_cpy{args.cbegin(), args.cend() - defaulted}; + std::vector args_type_cpy{args_type.cbegin(), + args_type.cend() - defaulted}; + std::vector args_names_cpy{args_names.cbegin(), + args_names.cend() - defaulted}; + header.funcs.emplace_back(std::make_shared( + func->name_, ret_type, std::move(args_cpy), std::move(args_type_cpy), + std::move(args_names_cpy))); + } header.funcs.emplace_back( std::make_shared(func->name_, ret_type, std::move(args), std::move(args_type), std::move(args_names))); } -inline void parse_namespace(Header& header, CppWriter& writer, - CppConstCompoundEPtr ns, std::string name) { - header.namespaces.emplace_back(name); +inline void parse_namespace(Header& header, CppConstCompoundEPtr ns, + std::string name) { + header.namespaces.insert(name); for (const CppObjPtr& obj : ns->members()) { CppVarEPtr var = obj; if (var) { @@ -187,7 +267,7 @@ inline void parse_namespace(Header& header, CppWriter& writer, } CppFunctionEPtr func = obj; if (func) { - parse_func(header, writer, func); + parse_func(header, func); continue; } CppEnumEPtr enu = obj; @@ -198,9 +278,9 @@ inline void parse_namespace(Header& header, CppWriter& writer, CppConstCompoundEPtr compound = obj; if (!compound) continue; if (isNamespace(compound)) - parse_namespace(header, writer, compound, name + "::" + compound->name()); + parse_namespace(header, compound, name + "::" + compound->name()); else if (isClass(compound) || isStruct(compound)) - parse_class(header, writer, compound); + parse_class(header, compound, name); } } @@ -211,7 +291,6 @@ inline void parse_header(Module& mod, fs::path path, const std::string& name) { parser.addIgnorableMacro(upper_name + "_LOCAL"); parser.addIgnorableMacro(upper_name + "_EXPORT"); parser.addIgnorableMacro(upper_name + "_IMPORT"); - CppWriter writer; mod.headers.emplace_back( std::make_shared
(name.substr(0, name.rfind('.')))); @@ -222,6 +301,6 @@ inline void parse_header(Module& mod, fs::path path, const std::string& name) { for (const CppObjPtr& obj_ns : ast->members()) { CppConstCompoundEPtr ns = obj_ns; if (!ns || !isNamespace(ns) || ns->name() != mod.name) continue; - parse_namespace(*mod.headers.back(), writer, ns, ns->name()); + parse_namespace(*mod.headers.back(), ns, ns->name()); } } diff --git a/src/ros2_control_py_builder/structs.hpp b/src/ros2_control_py_builder/structs.hpp index a1647c6..f8f53f7 100644 --- a/src/ros2_control_py_builder/structs.hpp +++ b/src/ros2_control_py_builder/structs.hpp @@ -3,6 +3,7 @@ // STL #include #include +#include #include // boost #include @@ -113,9 +114,11 @@ struct Memb : public Func { struct Header; struct Cls { - Cls(const Header& header, const std::string& name, std::string mother_name, + Cls(Header& header, const std::string& ns, const std::string& name, + const std::string& mother_name, std::shared_ptr mother = nullptr) : header{header}, + complete_name{ns + "::" + name}, name{name}, tramp_name("Py" + name), pub_name("Pub" + name), @@ -186,7 +189,8 @@ struct Cls { return found; } - const Header& header; + Header& header; + const std::string complete_name; const std::string name; const std::string tramp_name; const std::string pub_name; @@ -201,6 +205,7 @@ struct Cls { bool has_virtual{false}; bool has_pure{false}; bool has_protected{false}; + bool is_outsider{false}; }; struct Enum { @@ -241,12 +246,15 @@ struct Header { const std::string name; const std::string proper_name; - std::vector namespaces{}; + std::unordered_set namespaces{}; std::vector> classes{}; std::vector enums{}; std::vector vars{}; std::vector> funcs{}; - std::vector required; + std::vector required{}; + std::unordered_set< + std::tuple> + stl_bind{}; }; struct Module { @@ -254,11 +262,13 @@ struct Module { : inc_dir{inc_dir / name}, src_dir{src_dir / name}, src{src_dir / (name + "_py.cpp")}, - name{name} {} + name{name}, + py_utils{new Header{"py_utils"}} {} const fs::path inc_dir; const fs::path src_dir; const fs::path src; std::string name; std::vector> headers{}; + std::shared_ptr
py_utils; }; diff --git a/src/ros2_control_py_builder/utils.hpp b/src/ros2_control_py_builder/utils.hpp index 9def775..ca19ee2 100644 --- a/src/ros2_control_py_builder/utils.hpp +++ b/src/ros2_control_py_builder/utils.hpp @@ -11,6 +11,7 @@ #include #include #include +#include // CppParser #include #include @@ -56,23 +57,47 @@ inline std::string just_name(std::string&& name); inline void remove_attributes(std::string& contents) { // remove attributes aka [[...]] - auto it = contents.begin(); - while (it != contents.end()) { - it = std::search_n(it, contents.end(), 2, '['); - auto end = std::search_n(it, contents.end(), 2, ']'); - if (end != contents.end()) it = contents.erase(it, end + 2); + auto it = contents.cbegin(); + while (it != contents.cend()) { + it = std::search_n(it, contents.cend(), 2, '['); + auto end = std::search_n(it, contents.cend(), 2, ']'); + if (end != contents.cend()) it = contents.erase(it, end + 2); } - // digit separators aka d'ddd'ddd but not char digits aka 'd' - it = contents.begin(); - while (it != contents.end()) { - it = std::adjacent_find(it, contents.end(), [](char a, char b) { + // remove digit separators aka d'ddd'ddd but not char digits aka 'd' + it = contents.cbegin(); + while (it != contents.cend()) { + it = std::adjacent_find(it, contents.cend(), [](char a, char b) { return std::isdigit(a) && b == '\''; }); - if (it == contents.end()) continue; + if (it == contents.cend()) continue; it += 2; - if (it == contents.end() || !std::isdigit(*it)) continue; + if (it == contents.cend() || !std::isdigit(*it)) continue; it = contents.erase(it - 1); } + // remove raw strings aka R"()" + auto mit = contents.begin(); + while (mit != contents.end()) { + mit = std::adjacent_find(mit, contents.end(), [](char a, char b) { + return a == 'R' && b == '"'; + }); + if (mit == contents.end()) continue; + mit += 2; + if (mit == contents.end()) continue; + auto name_end = std::find(mit, contents.end(), '('); + std::string name = std::string{mit, name_end}; + auto valid_dchar = [](char c) { + return (!std::isalnum(c) && !std::ispunct(c)) || (c == ')' || c == '\\'); + }; + if (name.size() > 16 || + std::find_if(name.cbegin(), name.cend(), valid_dchar) != name.cend()) + continue; + std::size_t end = + contents.find(")" + name + "\"", name_end - contents.cbegin()); + if (end == std::string::npos) continue; + *(mit - 2) = '"'; // replpace R with " + mit = contents.erase( + mit - 1, contents.cbegin() + end + name.size() + 1); // R|"name()name|" + } } inline std::string read_file(const std::string& filename) { @@ -127,10 +152,15 @@ std::ostream& operator<<(std::ostream& os, const Sep& sep) { return os; } -inline std::string str_of_cpp(CppWriter& writer, const CppObj* cppObj) { +inline std::string str_of_cpp(const CppObj* cppObj) { std::ostringstream oss; - writer.emit(cppObj, oss); - return std::move(oss).str(); + CppWriter{}.emit(cppObj, oss); + std::string str = std::move(oss).str(); + if (!str.empty() && str.back() == '*') { + std::string type = str.substr(0, str.size() - 1); + if (type == "double") return "Ref<" + type + ">"; + } + return str; } inline std::string to_upper(std::string str) { @@ -139,6 +169,32 @@ inline std::string to_upper(std::string str) { return str; } +inline std::string to_pascal_case(std::string_view str) { + std::string r; + r.resize(str.size()); + auto rit = r.begin(); + for (auto it = str.cbegin(); it != str.cend(); ++it) { + auto end = std::find(it, str.cend(), '_'); + if (it == end) continue; + *rit++ = std::toupper(*it++); + if (it != end) { + std::copy(it, end, rit); + rit += end - it; + } + it = end; + if (it == str.cend()) break; + } + if (rit != r.cend()) r.erase(rit, r.cend()); + return r; +} + +template +inline std::string make_pascal_name(const Args&... args) { + std::string r; + (r.append(to_pascal_case(just_name(args))), ...); + return r; +} + inline std::string just_name(const std::string& name) { return just_name(std::string{name}); } @@ -277,3 +333,58 @@ template auto ptr_iter(const T& iterable) { return CPtrIterable{iterable}; } + +inline std::uint64_t fn(std::uint64_t x) { + const std::uint64_t m = (std::uint64_t(0xe9846af) << 32) + 0x9b1a615d; + + x ^= x >> 32; + x *= m; + x ^= x >> 32; + x *= m; + x ^= x >> 28; + + return x; +} + +inline std::uint32_t hash_mix(std::uint32_t x) { + const std::uint32_t m1 = 0x21f0aaad; + const std::uint32_t m2 = 0x735a2d97; + + x ^= x >> 16; + x *= m1; + x ^= x >> 15; + x *= m2; + x ^= x >> 15; + + return x; +} + +template +void combine_hash(std::size_t& seed, const T& data) { + seed = hash_mix(seed + 0x9e3779b9 + std::hash{}(data)); +} + +template +struct std::hash> { + constexpr std::uint64_t operator()(const std::pair& pair) const { + std::size_t h = std::hash{}(pair.first); + combine_hash(h, pair.second); + return h; + } +}; + +template +struct std::hash> { + constexpr std::uint64_t operator()(const std::tuple& tuple) const { + return impl(tuple, std::index_sequence_for{}); + } + + private: + template + constexpr std::uint64_t impl(const std::tuple& tuple, + std::index_sequence) const { + std::size_t h = 0; + (combine_hash(h, std::get(tuple)), ...); + return h; + } +}; diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index 060284b..652adb8 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -28,8 +28,11 @@ inline void write_module(const Module& mod); // Impl inline std::ostream& operator<<(std::ostream& os, const Cls& cls) { - os << " py::class_<" << cls.name; - if (cls.mother) os << ", " << cls.mother->name; + os << " py::class_<" << (cls.is_outsider ? cls.complete_name : cls.name); + if (cls.mother) + os << ", " + << (cls.mother->is_outsider ? cls.mother->complete_name + : cls.mother->name); if (cls.has_virtual) os << ", " << cls.tramp_name; os << ">(m, \"" << cls.name << "\")"; std::vector ctors = cls.ctors; @@ -39,19 +42,22 @@ inline std::ostream& operator<<(std::ostream& os, const Cls& cls) { for (const Memb& memb : ptr_iter(cls.membs)) { if (memb.is_overloaded) os << "\n .def(\"" << memb.name << "\", [](" - << (memb.is_public ? cls.name : cls.pub_name) << "& py_self" - << (memb.args.empty() ? "" : ", ") << Sep{memb.args, ", "} - << ") { return py_self." << memb.name << '(' + << (memb.is_public ? (cls.is_outsider ? cls.complete_name : cls.name) + : cls.pub_name) + << "& py_self" << (memb.args.empty() ? "" : ", ") + << Sep{memb.args, ", "} << ") { return py_self." << memb.name << '(' << Sep{memb.args_names, ", "} << "); })"; else os << "\n .def(\"" << memb.name << "\", &" - << (memb.is_public ? cls.name : cls.pub_name) << "::" << memb.name - << ")"; + << (memb.is_public ? (cls.is_outsider ? cls.complete_name : cls.name) + : cls.pub_name) + << "::" << memb.name << ")"; } for (const Attr& attr : ptr_iter(cls.attrs)) os << "\n .def_readwrite(\"" << attr.name << "\", &" - << (attr.is_public ? cls.name : cls.pub_name) << "::" << attr.name - << ")"; + << (attr.is_public ? (cls.is_outsider ? cls.complete_name : cls.name) + : cls.pub_name) + << "::" << attr.name << ')'; os << ";\n"; return os; } @@ -86,8 +92,7 @@ inline std::ostream& operator<<(std::ostream& os, const Enum& enu) { } inline std::ostream& operator<<(std::ostream& os, const Var& var) { - return os << " m.def(\"" << var.name << "\", []() { return " << var.name - << "; });\n"; + return os << " m.attr(\"" << var.name << "\") = " << var.name << ";\n"; } inline std::ostream& operator<<(std::ostream& os, const Func& func) { @@ -108,11 +113,81 @@ void write_module_header(const Module& mod, const Header& header) { // pybind11 #include -// )" << mod.name - << R"( -#include <)" - << mod.name << R"(/)" << header.name << R"(.hpp> +)"; + if (header.name != mod.py_utils->name) + ofs << "\n// " << mod.name << "\n#include <" << mod.name << '/' + << header.name << ".hpp>\n// py_utils\n#include <" << mod.name << '/' + << mod.py_utils->name << "_py.hpp>\n"; + else { + ofs << "#include \n// STL\n#include " + "\n#include " + "\n#include \n#include \n// " + << mod.name << '\n'; + for (const Header& h : ptr_iter(mod.headers)) + if (&h != &header) + ofs << "#include <" << mod.name << '/' << h.name << ".hpp>\n"; + ofs << "\nnamespace PYBIND11_NAMESPACE {\nnamespace detail {\n"; + for (const std::string& ns : header.namespaces) + ofs << "using namespace " << ns << ";\n"; + ofs << "}\n}\n"; + if (!header.stl_bind.empty()) ofs << '\n'; + for (const auto& [type, cpp_type, u, v] : header.stl_bind) + ofs << "PYBIND11_MAKE_OPAQUE(std::" << cpp_type << '<' << u + << (v.empty() ? "" : ", " + v) << ">);\n"; + ofs << R"( +namespace PYBIND11_NAMESPACE { +namespace detail { +template +struct type_caster> { + public: + PYBIND11_TYPE_CASTER(std::unique_ptr, const_name("UniqueT")); + + bool load(handle src, bool) { + try { + T* data = src.cast(); + src.inc_ref(); + value.reset(data); + } catch (cast_error&) { + return false; + } + return true; + } + + static handle cast(std::unique_ptr /* src */, return_value_policy /* policy */, handle /* parent */) { + throw std::runtime_error("invalid unique_ptr to python"); + } +}; +template +struct type_caster> { + public: + PYBIND11_TYPE_CASTER(std::vector, const_name("VectorT")); + + bool load(handle src, bool) { + list l = reinterpret_borrow(src); + value.clear(); + try { + for (const handle& el: l) + value.emplace_back(std::move(el.cast())); + } catch (cast_error&) { + return false; + } + return true; + } + static handle cast(std::vector&& src, return_value_policy /* policy */, handle /* parent */) { + list l{0}; + l.inc_ref(); + for (T& el: src) + l.append(std::move(el)); + src.clear(); + return l; + } +}; +} // namespace detail +} // namespace PYBIND11_NAMESPACE +)"; + } + ofs << R"( namespace ros2_control_py::bind_)" << mod.name << R"( { @@ -123,28 +198,183 @@ namespace py = pybind11; ofs << "using namespace " << ns << ";\n"; for (const Cls& cls : ptr_iter(header.classes)) { if (!cls.has_virtual) continue; - ofs << "\nclass " << cls.tramp_name << ": public " << cls.name + ofs << "\nclass " << cls.tramp_name << ": public " + << (cls.is_outsider ? cls.complete_name : cls.name) << " {\n public:\n using " << cls.name << "::" << cls.name << ";\n"; for (const Memb& vmemb : ptr_iter(cls.find_vmembs())) ofs << vmemb; ofs << "};\n"; } for (const Cls& cls : ptr_iter(header.classes)) { if (!cls.has_protected) continue; - ofs << "\nclass " << cls.pub_name << ": public " << cls.name - << " {\n public:\n"; + ofs << "\nclass " << cls.pub_name << ": public " + << (cls.is_outsider ? cls.complete_name : cls.name) << " {\n public:\n"; for (const Memb& pmemb : ptr_iter(cls.find_pmembs())) ofs << " using " << cls.name << "::" << pmemb.name << ";\n"; for (const Attr& pattr : ptr_iter(cls.find_pattrs())) ofs << " using " << cls.name << "::" << pattr.name << ";\n"; ofs << "};\n"; } + if (header.name == mod.py_utils->name) { + ofs << R"( +template +class Ref { + public: + Ref() : value_{std::make_shared()} {} + Ref(const T& value) : value_{std::make_shared(value)} {} + + operator T*() const { return value_.get(); } + operator T&() const { return *value_; } + + T& operator*() const { return *value_; } + T* operator->() const { return value_.get(); } + + T value() const { return *value_; } + + std::string str() const { + std::ostringstream oss; + oss << *value_; + return std::move(oss).str(); + } + + private: + std::shared_ptr value_; +}; + +template +class RefProp { + public: + RefProp(const T& default_value) : default_value_{default_value} {} + + Ref get(py::object instance, py::object) { + std::size_t id = reinterpret_cast(instance.ptr()); + auto it = values_.find(id); + if (it == values_.end()) + it = values_.insert({id, Ref(default_value_)}).first; + return it->second; + } + + void set(py::object instance, const T& value) { + std::size_t id = reinterpret_cast(instance.ptr()); + auto it = values_.find(id); + if (it != values_.end()) + *it->second = value; + else + values_.insert({id, Ref(value)}); + } + + void del(py::object) {} + + private: + T default_value_; + std::unordered_map> values_; +}; +)"; + } ofs << R"( inline void init_)" << header.proper_name << R"((py::module &m) { -)" << Sep{header.vars, "\n"} - << Sep{ptr_iter(header.funcs), "\n"} << Sep{header.enums, "\n"} - << Sep{ptr_iter(header.classes), "\n"} << R"(} +)"; + if (header.name == mod.py_utils->name) { + ofs << R"( py::class_>(m, "FloatRef") + .def(py::init()) + .def("__repr__", &Ref::str) + .def("value", &Ref::value) + .def("__add__", [](const Ref& lhs, const Ref& rhs) { return *lhs + *rhs; }) + .def("__add__", [](const Ref& lhs, double rhs) { return *lhs + rhs; }) + .def("__add__", [](const Ref& lhs, long long rhs) { return *lhs + rhs; }) + .def("__radd__", [](const Ref& rhs, double lhs) { return lhs + *rhs; }) + .def("__radd__", [](const Ref& rhs, long long lhs) { return lhs + *rhs; }) + .def("__iadd__", [](Ref& lhs, const Ref& rhs) { *lhs += *rhs; return lhs; }) + .def("__iadd__", [](Ref& lhs, double rhs) { *lhs += rhs; return lhs; }) + .def("__iadd__", [](Ref& lhs, long long rhs) { *lhs += rhs; return lhs; }) + .def("__sub__", [](const Ref& lhs, const Ref& rhs) { return *lhs - *rhs; }) + .def("__sub__", [](const Ref& lhs, double rhs) { return *lhs - rhs; }) + .def("__sub__", [](const Ref& lhs, long long rhs) { return *lhs - rhs; }) + .def("__rsub__", [](const Ref& rhs, double lhs) { return lhs - *rhs; }) + .def("__rsub__", [](const Ref& rhs, long long lhs) { return lhs - *rhs; }) + .def("__isub__", [](Ref& lhs, const Ref& rhs) { *lhs -= *rhs; return lhs; }) + .def("__isub__", [](Ref& lhs, double rhs) { *lhs -= rhs; return lhs; }) + .def("__isub__", [](Ref& lhs, long long rhs) { *lhs -= rhs; return lhs; }) + .def("__mul__", [](const Ref& lhs, const Ref& rhs) { return *lhs * *rhs; }) + .def("__mul__", [](const Ref& lhs, double rhs) { return *lhs * rhs; }) + .def("__mul__", [](const Ref& lhs, long long rhs) { return *lhs * rhs; }) + .def("__rmul__", [](const Ref& rhs, double lhs) { return lhs * *rhs; }) + .def("__rmul__", [](const Ref& rhs, long long lhs) { return lhs * *rhs; }) + .def("__imul__", [](Ref& lhs, const Ref& rhs) { *lhs *= *rhs; return lhs; }) + .def("__imul__", [](Ref& lhs, double rhs) { *lhs *= rhs; return lhs; }) + .def("__imul__", [](Ref& lhs, long long rhs) { *lhs *= rhs; return lhs; }) + .def("__div__", [](const Ref& lhs, const Ref& rhs) { return *lhs / *rhs; }) + .def("__div__", [](const Ref& lhs, double rhs) { return *lhs / rhs; }) + .def("__div__", [](const Ref& lhs, long long rhs) { return *lhs / rhs; }) + .def("__rdiv__", [](const Ref& rhs, double lhs) { return lhs / *rhs; }) + .def("__rdiv__", [](const Ref& rhs, long long lhs) { return lhs / *rhs; }) + .def("__idiv__", [](Ref& lhs, const Ref& rhs) { *lhs /= *rhs; return lhs; }) + .def("__idiv__", [](Ref& lhs, double rhs) { *lhs /= rhs; return lhs; }) + .def("__idiv__", [](Ref& lhs, long long rhs) { *lhs /= rhs; return lhs; }); + + py::class_>(m, "FloatRefProp") + .def(py::init()) + .def("__get__", &RefProp::get) + .def("__set__", &RefProp::set) + .def("__set__", [](RefProp& py_self, py::object instance, long long value) { py_self.set(instance, value); }) + .def("__set__", [](RefProp& py_self, py::object instance, Ref value) { py_self.set(instance, value); }) + .def("__delete__", &RefProp::del); + +)"; + if (mod.name == "hardware_interface") { + ofs << R"( py::enum_(m, "CallbackReturn") + .value("SUCCESS", CallbackReturn::SUCCESS) + .value("FAILURE", CallbackReturn::FAILURE) + .value("ERROR", CallbackReturn::ERROR) + .export_values(); + + py::class_(m, "State") + .def(py::init<>()) + .def(py::init()) + .def("id", &rclcpp_lifecycle::State::id) + .def("label", &rclcpp_lifecycle::State::label); + + py::class_(m, "Time") + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def("nanoseconds", &rclcpp::Time::nanoseconds) + .def("__eq__", [](const rclcpp::Time& lhs, const rclcpp::Time& rhs) { return lhs == rhs; }) + .def("__ne__", [](const rclcpp::Time& lhs, const rclcpp::Time& rhs) { return lhs != rhs; }) + .def("__ge__", [](const rclcpp::Time& lhs, const rclcpp::Time& rhs) { return lhs >= rhs; }) + .def("__gt__", [](const rclcpp::Time& lhs, const rclcpp::Time& rhs) { return lhs > rhs; }) + .def("__le__", [](const rclcpp::Time& lhs, const rclcpp::Time& rhs) { return lhs <= rhs; }) + .def("__lt__", [](const rclcpp::Time& lhs, const rclcpp::Time& rhs) { return lhs < rhs; }) + .def("__add__", [](const rclcpp::Time& lhs, const rclcpp::Duration& rhs) { return lhs + rhs; }) + .def("__sub__", [](const rclcpp::Time& lhs, const rclcpp::Time& rhs) { return lhs - rhs; }) + .def("__sub__", [](const rclcpp::Time& lhs, const rclcpp::Duration& rhs) { return lhs - rhs; }); + + py::class_(m, "Duration") + .def(py::init()) + .def("nanoseconds", &rclcpp::Duration::nanoseconds) + .def("__eq__", [](const rclcpp::Duration& lhs, const rclcpp::Duration& rhs) { return lhs == rhs; }) + .def("__ne__", [](const rclcpp::Duration& lhs, const rclcpp::Duration& rhs) { return lhs != rhs; }) + .def("__ge__", [](const rclcpp::Duration& lhs, const rclcpp::Duration& rhs) { return lhs >= rhs; }) + .def("__gt__", [](const rclcpp::Duration& lhs, const rclcpp::Duration& rhs) { return lhs > rhs; }) + .def("__le__", [](const rclcpp::Duration& lhs, const rclcpp::Duration& rhs) { return lhs <= rhs; }) + .def("__lt__", [](const rclcpp::Duration& lhs, const rclcpp::Duration& rhs) { return lhs < rhs; }) + .def("__add__", [](const rclcpp::Duration& lhs, const rclcpp::Duration& rhs) { return lhs + rhs; }) + .def("__sub__", [](const rclcpp::Duration& lhs, const rclcpp::Duration& rhs) { return lhs - rhs; }) + .def("__mul__", [](const rclcpp::Duration& lhs, double rhs) { return lhs * rhs; }); + +)"; + } + } + for (const auto& [type, cpp_type, u, v] : header.stl_bind) { + std::string complete_type = + "std::" + cpp_type + "<" + u + (v.empty() ? "" : ", " + v) + ">"; + ofs << " py::bind_" << type << '<' << complete_type << ">(m, \"" + << make_pascal_name(cpp_type, u, v) << "\");\n"; + } + if (!header.stl_bind.empty()) ofs << '\n'; + ofs << Sep{header.vars, "\n"} << Sep{ptr_iter(header.funcs), "\n"} + << Sep{header.enums, "\n"} << Sep{ptr_iter(header.classes), "\n"} << R"(} } )"; @@ -162,13 +392,12 @@ void write_module(const Module& mod) { // )" << mod.name << '\n'; for (const Header& header : ptr_iter(mod.headers)) - ofs << "#include \"" << mod.name << "/" << header.name << "_py.hpp\"\n"; + ofs << "#include <" << mod.name << "/" << header.name << "_py.hpp>\n"; ofs << R"( namespace py = pybind11; PYBIND11_MODULE()" - << mod.name + "_py" - << R"(, m) + << mod.name << R"(, m) { m.doc() = R"doc( Python bindings for ros2_control functionalities. diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/hardware_interface/__init__.py b/tests/hardware_interface/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/hardware_interface/test_component_interface.py b/tests/hardware_interface/test_component_interface.py new file mode 100644 index 0000000..908e115 --- /dev/null +++ b/tests/hardware_interface/test_component_interface.py @@ -0,0 +1,201 @@ +from ros2_control_py.hardware_interface import ( + Actuator, + ActuatorInterface, + CommandInterface, + StateInterface, + HardwareInfo, + HW_IF_POSITION, + HW_IF_VELOCITY, + UNCONFIGURED, + ACTIVE, + INACTIVE, + FINALIZED, + return_type, + FloatRefProp, + CallbackReturn, + Time, + Duration, + VectorString, +) +from lifecycle_msgs.msg import State +from math import isnan, nan + +TIME = Time(0) +PERIOD = Duration(0, 10000000) +TRIGGER_READ_WRITE_ERROR_CALLS = 10000 + + +class DummyActuator(ActuatorInterface): + position_state_ = FloatRefProp(nan) + velocity_state_ = FloatRefProp(nan) + velocity_command_ = FloatRefProp(0.0) + + def __init__(self): + super().__init__() + self.read_calls_ = 0 + self.write_calls_ = 0 + self.recoverable_error_happened_ = False + + def on_init(self, info): + return CallbackReturn.SUCCESS + + def on_configure(self, previous_state): + self.position_state_ = 0.0 + self.velocity_state_ = 0.0 + if self.recoverable_error_happened_: + velocity_command_ = 0.0 + self.read_calls_ = 0 + self.write_calls_ = 0 + return CallbackReturn.SUCCESS + + def export_state_interfaces(self): + state_interfaces = [] + state_interfaces.append( + StateInterface("joint1", HW_IF_POSITION, self.position_state_) + ) + state_interfaces.append( + StateInterface("joint1", HW_IF_VELOCITY, self.velocity_state_) + ) + return state_interfaces + + def export_command_interfaces(self): + # We can command in velocity + command_interfaces = [] + command_interfaces.append( + CommandInterface("joint1", HW_IF_VELOCITY, self.velocity_command_) + ) + return command_interfaces + + def get_name(self): + return "DummyActuator" + + def read(self, time, period): + self.read_calls_ += 1 + if self.read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS: + return return_type.ERROR + # no-op, state is getting propagated within write. + return return_type.OK + + def write(self, time, period): + self.write_calls_ += 1 + if self.write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS: + return return_type.ERROR + self.position_state_ += self.velocity_command_ + self.velocity_state_ = self.velocity_command_ + return return_type.OK + + def on_shutdown(self, previous_state): + self.velocity_state_ = 0 + return CallbackReturn.SUCCESS + + def on_error(self, previous_state): + if not self.recoverable_error_happened_: + self.recoverable_error_happened_ = True + return CallbackReturn.SUCCESS + else: + return CallbackReturn.ERROR + return CallbackReturn.FAILURE + + +def ASSERT_TRUE(a): + assert a + + +def ASSERT_EQ(a, b): + assert a == b + + +def EXPECT_EQ(a, b): + ASSERT_EQ(a, b) + + +def test_dummy_actuator(): + dummy_actuator_hw = DummyActuator() + actuator_hw = Actuator(dummy_actuator_hw) + + mock_hw_info = HardwareInfo() + state = actuator_hw.initialize(mock_hw_info) + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + state_interfaces = actuator_hw.export_state_interfaces() + ASSERT_EQ(2, len(state_interfaces)) + EXPECT_EQ("joint1/position", state_interfaces[0].get_name()) + EXPECT_EQ(HW_IF_POSITION, state_interfaces[0].get_interface_name()) + EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()) + EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()) + EXPECT_EQ(HW_IF_VELOCITY, state_interfaces[1].get_interface_name()) + EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()) + + command_interfaces = actuator_hw.export_command_interfaces() + ASSERT_EQ(1, len(command_interfaces)) + EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()) + EXPECT_EQ(HW_IF_VELOCITY, command_interfaces[0].get_interface_name()) + EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()) + + velocity_value = 1.0 + command_interfaces[0].set_value(velocity_value) + ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + + # Noting should change because it is UNCONFIGURED + for step in range(10): + ASSERT_EQ(return_type.ERROR, actuator_hw.read(TIME, PERIOD)) + + ASSERT_TRUE(isnan(state_interfaces[0].get_value())) # position value + ASSERT_TRUE(isnan(state_interfaces[1].get_value())) # velocity + + ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + + state = actuator_hw.configure() + EXPECT_EQ(State.PRIMARY_STATE_INACTIVE, state.id()) + EXPECT_EQ(INACTIVE, state.label()) + + # Read and Write are working because it is INACTIVE + for step in range(10): + ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + + EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()) + # position value + EXPECT_EQ(velocity_value if step else 0, state_interfaces[1].get_value()) + # velocity + + ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + + state = actuator_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + # Read and Write are working because it is ACTIVE + for step in range(10): + ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + + EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()) + # position value + EXPECT_EQ(velocity_value, state_interfaces[1].get_value()) + # velocity + + ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + + state = actuator_hw.shutdown() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + # Noting should change because it is FINALIZED + for step in range(10): + ASSERT_EQ(return_type.ERROR, actuator_hw.read(TIME, PERIOD)) + + EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()) + # position value + EXPECT_EQ(0, state_interfaces[1].get_value()) + # velocity + + ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + + EXPECT_EQ( + return_type.OK, + actuator_hw.prepare_command_mode_switch(VectorString([""]), VectorString([""])), + ) + EXPECT_EQ( + return_type.OK, + actuator_hw.perform_command_mode_switch(VectorString([""]), VectorString([""])), + ) diff --git a/tests/hardware_interface/test_component_parser.py b/tests/hardware_interface/test_component_parser.py new file mode 100644 index 0000000..839ca29 --- /dev/null +++ b/tests/hardware_interface/test_component_parser.py @@ -0,0 +1,715 @@ +from ros2_control_py.hardware_interface import ( + HW_IF_EFFORT, + HW_IF_POSITION, + HW_IF_VELOCITY, + parse_control_resources_from_urdf, +) +from ros2_control_py import ros2_control_test_assets +import pytest +from locale import setlocale +from math import isclose, pi +from tests.utils import ROS_DISTRO, hardware_plugin_name + + +def test_empty_string_throws_error(): + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf("") + + +def test_empty_urdf_throws_error(): + empty_urdf = ( + '' + ) + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(empty_urdf) + + +def test_string_robot_not_root_throws_error(): + broken_xml_string = """ + > + """ + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(broken_xml_string) + + +def test_invalid_child_throws_error(): + broken_urdf_string = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.invalid_urdf_ros2_control_invalid_child + + ros2_control_test_assets.urdf_tail + ) + + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(broken_urdf_string) + + +def test_missing_attribute_throws_error(): + broken_urdf_string = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.invalid_urdf_ros2_control_missing_attribute + + ros2_control_test_assets.urdf_tail + ) + + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(broken_urdf_string) + + +def test_parameter_missing_name_throws_error(): + broken_urdf_string = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.invalid_urdf_ros2_control_parameter_missing_name + + ros2_control_test_assets.urdf_tail + ) + + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(broken_urdf_string) + + +def test_component_interface_type_empty_throws_error(): + broken_urdf_string = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.invalid_urdf_ros2_control_component_interface_type_empty + + ros2_control_test_assets.urdf_tail + ) + + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(broken_urdf_string) + + +def test_successfully_parse_valid_urdf_system_one_interface(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.name == "RRBotSystemPositionOnly" + assert hardware_info.type == "system" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "2" + + assert len(hardware_info.joints) == 2 + + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + assert len(hardware_info.joints[0].command_interfaces) == 1 + assert hardware_info.joints[0].command_interfaces[0].name == HW_IF_POSITION + assert hardware_info.joints[0].command_interfaces[0].min == "-1" + assert hardware_info.joints[0].command_interfaces[0].max == "1" + assert len(hardware_info.joints[0].state_interfaces) == 1 + assert hardware_info.joints[0].state_interfaces[0].name == HW_IF_POSITION + + assert hardware_info.joints[1].name == "joint2" + assert hardware_info.joints[1].type == "joint" + assert len(hardware_info.joints[1].command_interfaces) == 1 + assert hardware_info.joints[1].command_interfaces[0].name == HW_IF_POSITION + assert hardware_info.joints[1].command_interfaces[0].min == "-1" + assert hardware_info.joints[1].command_interfaces[0].max == "1" + assert len(hardware_info.joints[1].state_interfaces) == 1 + assert hardware_info.joints[1].state_interfaces[0].name == HW_IF_POSITION + + +def test_successfully_parse_valid_urdf_system_multi_interface(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_system_multi_interface + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.name == "RRBotSystemMultiInterface" + assert hardware_info.type == "system" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/RRBotSystemMultiInterfaceHardware" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "2" + assert hardware_info.hardware_parameters["example_param_read_for_sec"] == "2" + + assert len(hardware_info.joints) == 2 + + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + assert len(hardware_info.joints[0].command_interfaces) == 3 + assert hardware_info.joints[0].command_interfaces[0].name == HW_IF_POSITION + assert hardware_info.joints[0].command_interfaces[0].initial_value == "1.2" + assert hardware_info.joints[0].command_interfaces[1].initial_value == "3.4" + assert len(hardware_info.joints[0].state_interfaces) == 3 + assert hardware_info.joints[0].state_interfaces[1].name == HW_IF_VELOCITY + + assert hardware_info.joints[1].name == "joint2" + assert hardware_info.joints[1].type == "joint" + assert len(hardware_info.joints[1].command_interfaces) == 1 + assert len(hardware_info.joints[1].state_interfaces) == 3 + assert hardware_info.joints[1].state_interfaces[2].name == HW_IF_EFFORT + + +def test_successfully_parse_valid_urdf_system_robot_with_sensor(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_system_robot_with_sensor + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.name == "RRBotSystemWithSensor" + assert hardware_info.type == "system" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/RRBotSystemWithSensorHardware" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "2" + + assert len(hardware_info.joints) == 2 + + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + + assert hardware_info.joints[1].name == "joint2" + assert hardware_info.joints[1].type == "joint" + + assert len(hardware_info.sensors) == 1 + + assert hardware_info.sensors[0].name == "tcp_fts_sensor" + assert hardware_info.sensors[0].type == "sensor" + assert len(hardware_info.sensors[0].state_interfaces) == 6 + assert len(hardware_info.sensors[0].command_interfaces) == 0 + assert hardware_info.sensors[0].state_interfaces[0].name == "fx" + assert hardware_info.sensors[0].state_interfaces[1].name == "fy" + assert hardware_info.sensors[0].state_interfaces[2].name == "fz" + assert hardware_info.sensors[0].state_interfaces[3].name == "tx" + assert hardware_info.sensors[0].state_interfaces[4].name == "ty" + assert hardware_info.sensors[0].state_interfaces[5].name == "tz" + + assert len(hardware_info.sensors[0].parameters) == 3 + assert hardware_info.sensors[0].parameters["frame_id"] == "kuka_tcp" + assert hardware_info.sensors[0].parameters["lower_limits"] == "-100" + assert hardware_info.sensors[0].parameters["upper_limits"] == "100" + + +def test_successfully_parse_valid_urdf_system_robot_with_external_sensor(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_system_robot_with_external_sensor + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 2 + hardware_info = control_hardware[0] + + assert hardware_info.name == "RRBotSystemPositionOnlyWithExternalSensor" + assert hardware_info.type == "system" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "2" + + assert len(hardware_info.joints) == 2 + + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + + assert hardware_info.joints[1].name == "joint2" + assert hardware_info.joints[1].type == "joint" + + assert len(hardware_info.sensors) == 0 + + hardware_info = control_hardware[1] + + assert hardware_info.name == "RRBotForceTorqueSensor2D" + assert hardware_info.type == "sensor" + assert len(hardware_info.hardware_parameters) == 1 + assert hardware_info.hardware_parameters["example_param_read_for_sec"] == "0.43" + + assert len(hardware_info.sensors) == 1 + assert hardware_info.sensors[0].name == "tcp_fts_sensor" + assert hardware_info.sensors[0].type == "sensor" + assert hardware_info.sensors[0].parameters["frame_id"] == "kuka_tcp" + + +def test_successfully_parse_valid_urdf_actuator_modular_robot(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_actuator_modular_robot + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 2 + hardware_info = control_hardware[0] + + assert hardware_info.name == "RRBotModularJoint1" + assert hardware_info.type == "actuator" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/PositionActuatorHardware" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "1.23" + + assert len(hardware_info.joints) == 1 + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + + hardware_info = control_hardware[1] + + assert hardware_info.name == "RRBotModularJoint2" + assert hardware_info.type == "actuator" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/PositionActuatorHardware" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_read_for_sec"] == "3" + + assert len(hardware_info.joints) == 1 + assert hardware_info.joints[0].name == "joint2" + assert hardware_info.joints[0].type == "joint" + + +def test_successfully_parse_valid_urdf_actuator_modular_robot_with_sensors(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_actuator_modular_robot_sensors + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 4 + hardware_info = control_hardware[0] + + assert hardware_info.name == "RRBotModularJoint1" + assert hardware_info.type == "actuator" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/VelocityActuatorHardware" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "1.23" + + assert len(hardware_info.joints) == 1 + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + assert len(hardware_info.joints[0].command_interfaces) == 1 + assert hardware_info.joints[0].command_interfaces[0].name == HW_IF_VELOCITY + + assert len(hardware_info.transmissions) == 1 + assert hardware_info.transmissions[0].name == "transmission1" + assert ( + hardware_info.transmissions[0].type + == "transmission_interface/SimpleTansmission" + ) + assert len(hardware_info.transmissions[0].joints) == 1 + assert hardware_info.transmissions[0].joints[0].name == "joint1" + assert isclose( + hardware_info.transmissions[0].joints[0].mechanical_reduction, + 1024.0 / pi, + abs_tol=0.01, + ) + assert len(hardware_info.transmissions[0].actuators) == 1 + assert hardware_info.transmissions[0].actuators[0].name == "actuator1" + + hardware_info = control_hardware[1] + + assert hardware_info.name == "RRBotModularJoint2" + assert hardware_info.type == "actuator" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/VelocityActuatorHardware" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_read_for_sec"] == "3" + + assert len(hardware_info.joints) == 1 + assert hardware_info.joints[0].name == "joint2" + assert hardware_info.joints[0].type == "joint" + assert len(hardware_info.joints[0].command_interfaces) == 1 + assert hardware_info.joints[0].command_interfaces[0].name == HW_IF_VELOCITY + assert hardware_info.joints[0].command_interfaces[0].min == "-1" + assert hardware_info.joints[0].command_interfaces[0].max == "1" + + hardware_info = control_hardware[2] + + assert hardware_info.name == "RRBotModularPositionSensorJoint1" + assert hardware_info.type == "sensor" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/PositionSensorHardware" + ) + assert len(hardware_info.hardware_parameters) == 1 + assert hardware_info.hardware_parameters["example_param_read_for_sec"] == "2" + + assert len(hardware_info.sensors) == 0 + assert len(hardware_info.joints) == 1 + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + assert len(hardware_info.joints[0].command_interfaces) == 0 + assert len(hardware_info.joints[0].state_interfaces) == 1 + assert hardware_info.joints[0].state_interfaces[0].name == HW_IF_POSITION + + hardware_info = control_hardware[3] + + assert hardware_info.name == "RRBotModularPositionSensorJoint2" + assert hardware_info.type == "sensor" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/PositionSensorHardware" + ) + assert len(hardware_info.hardware_parameters) == 1 + assert hardware_info.hardware_parameters["example_param_read_for_sec"] == "2" + + assert len(hardware_info.sensors) == 0 + assert len(hardware_info.joints) == 1 + assert hardware_info.joints[0].name == "joint2" + assert hardware_info.joints[0].type == "joint" + assert len(hardware_info.joints[0].command_interfaces) == 0 + assert len(hardware_info.joints[0].state_interfaces) == 1 + assert hardware_info.joints[0].state_interfaces[0].name == HW_IF_POSITION + + +def test_successfully_parse_valid_urdf_system_multi_joints_transmission(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_system_multi_joints_transmission + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.name == "RRBotModularWrist" + assert hardware_info.type == "system" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/ActuatorHardwareMultiDOF" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "1.23" + + assert len(hardware_info.joints) == 2 + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + assert hardware_info.joints[1].name == "joint2" + assert hardware_info.joints[1].type == "joint" + + assert len(hardware_info.transmissions) == 1 + assert hardware_info.transmissions[0].name == "transmission1" + assert ( + hardware_info.transmissions[0].type + == "transmission_interface/DifferentialTransmission" + ) + assert len(hardware_info.transmissions[0].joints) == 2 + assert hardware_info.transmissions[0].joints[0].name == "joint1" + assert hardware_info.transmissions[0].joints[0].role == "joint1" + assert hardware_info.transmissions[0].joints[0].mechanical_reduction == 10.0 + assert hardware_info.transmissions[0].joints[0].offset == 0.5 + assert hardware_info.transmissions[0].joints[1].name == "joint2" + assert hardware_info.transmissions[0].joints[1].role == "joint2" + assert hardware_info.transmissions[0].joints[1].mechanical_reduction == 50.0 + assert hardware_info.transmissions[0].joints[1].offset == 0.0 + + assert len(hardware_info.transmissions[0].actuators) == 2 + assert hardware_info.transmissions[0].actuators[0].name == "joint1_motor" + assert hardware_info.transmissions[0].actuators[0].role == "actuator1" + assert hardware_info.transmissions[0].actuators[1].name == "joint2_motor" + assert hardware_info.transmissions[0].actuators[1].role == "actuator2" + + +def test_successfully_parse_valid_urdf_sensor_only(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_sensor_only + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.name == "CameraWithIMU" + assert hardware_info.type == "sensor" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/CameraWithIMUSensor" + ) + assert len(hardware_info.hardware_parameters) == 1 + assert hardware_info.hardware_parameters["example_param_read_for_sec"] == "2" + + assert len(hardware_info.sensors) == 2 + assert hardware_info.sensors[0].name == "sensor1" + assert hardware_info.sensors[0].type == "sensor" + assert len(hardware_info.sensors[0].state_interfaces) == 3 + assert hardware_info.sensors[0].state_interfaces[0].name == "roll" + assert hardware_info.sensors[0].state_interfaces[1].name == "pitch" + assert hardware_info.sensors[0].state_interfaces[2].name == "yaw" + + assert hardware_info.sensors[1].name == "sensor2" + assert hardware_info.sensors[1].type == "sensor" + assert len(hardware_info.sensors[1].state_interfaces) == 1 + assert hardware_info.sensors[1].state_interfaces[0].name == "image" + + +def test_successfully_parse_valid_urdf_actuator_only(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_actuator_only + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.name == "ActuatorModularJoint1" + assert hardware_info.type == "actuator" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/VelocityActuatorHardware" + ) + assert len(hardware_info.hardware_parameters) == 2 + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "1.13" + + assert len(hardware_info.joints) == 1 + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + assert len(hardware_info.joints[0].command_interfaces) == 1 + assert hardware_info.joints[0].command_interfaces[0].name == HW_IF_VELOCITY + assert hardware_info.joints[0].command_interfaces[0].min == "-1" + assert hardware_info.joints[0].command_interfaces[0].max == "1" + assert len(hardware_info.joints[0].state_interfaces) == 1 + assert hardware_info.joints[0].state_interfaces[0].name == HW_IF_VELOCITY + + assert len(hardware_info.transmissions) == 1 + transmission = hardware_info.transmissions[0] + assert transmission.name == "transmission1" + assert transmission.type == "transmission_interface/RotationToLinerTansmission" + assert len(transmission.joints) == 1 + joint = transmission.joints[0] + assert joint.name == "joint1" + assert joint.role == "joint1" + assert "velocity" in joint.state_interfaces + assert "velocity" in joint.command_interfaces + assert joint.mechanical_reduction == 325.949 + assert joint.offset == 0.0 + assert len(transmission.actuators) == 1 + actuator = transmission.actuators[0] + assert actuator.name == "actuator1" + assert actuator.role == "actuator1" + assert actuator.state_interfaces == joint.state_interfaces + assert actuator.command_interfaces == joint.command_interfaces + assert actuator.offset == 0.0 + assert len(transmission.parameters) == 1 + assert transmission.parameters["additional_special_parameter"] == "1337" + + +def test_successfully_parse_locale_independent_double(): + if ROS_DISTRO == "humble": + return + # Set to locale with comma-separated decimals + setlocale(locale.LC_NUMERIC, ("fr_FR", "UTF-8")) + + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_actuator_only + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "1.13" + + assert len(hardware_info.transmissions) == 1 + transmission = hardware_info.transmissions[0] + assert len(transmission.joints) == 1 + joint = transmission.joints[0] + + # Test that we still parse doubles using dot notation + assert joint.mechanical_reduction == 325.949 + + +def test_successfully_parse_valid_urdf_system_robot_with_gpio(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_system_robot_with_gpio + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.name == "RRBotSystemWithGPIO" + assert hardware_info.type == "system" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware" + ) + + assert len(hardware_info.joints) == 2 + + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + + assert hardware_info.joints[1].name == "joint2" + assert hardware_info.joints[1].type == "joint" + + assert len(hardware_info.gpios) == 2 + + assert hardware_info.gpios[0].name == "flange_analog_IOs" + assert hardware_info.gpios[0].type == "gpio" + assert len(hardware_info.gpios[0].state_interfaces) == 3 + assert len(hardware_info.gpios[0].command_interfaces) == 1 + assert hardware_info.gpios[0].state_interfaces[0].name == "analog_output1" + assert hardware_info.gpios[0].state_interfaces[1].name == "analog_input1" + assert hardware_info.gpios[0].state_interfaces[2].name == "analog_input2" + + assert hardware_info.gpios[1].name == "flange_vacuum" + assert hardware_info.gpios[1].type == "gpio" + assert len(hardware_info.gpios[1].state_interfaces) == 1 + assert len(hardware_info.gpios[1].command_interfaces) == 1 + assert hardware_info.gpios[1].state_interfaces[0].name == "vacuum" + assert hardware_info.gpios[1].command_interfaces[0].name == "vacuum" + + assert len(hardware_info.transmissions) == 0 + + +def test_successfully_parse_valid_urdf_system_with_size_and_data_type(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_system_robot_with_size_and_data_type + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.name == "RRBotSystemWithSizeAndDataType" + assert hardware_info.type == "system" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/RRBotSystemWithSizeAndDataType" + ) + + assert len(hardware_info.joints) == 1 + + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + assert len(hardware_info.joints[0].command_interfaces) == 1 + assert hardware_info.joints[0].command_interfaces[0].name == HW_IF_POSITION + assert hardware_info.joints[0].command_interfaces[0].data_type == "double" + assert hardware_info.joints[0].command_interfaces[0].size == 1 + assert len(hardware_info.joints[0].state_interfaces) == 1 + assert hardware_info.joints[0].state_interfaces[0].name == HW_IF_POSITION + assert hardware_info.joints[0].state_interfaces[0].data_type == "double" + assert hardware_info.joints[0].state_interfaces[0].size == 1 + + assert len(hardware_info.gpios) == 1 + + assert hardware_info.gpios[0].name == "flange_IOS" + assert hardware_info.gpios[0].type == "gpio" + assert len(hardware_info.gpios[0].command_interfaces) == 1 + assert hardware_info.gpios[0].command_interfaces[0].name == "digital_output" + assert hardware_info.gpios[0].command_interfaces[0].data_type == "bool" + assert hardware_info.gpios[0].command_interfaces[0].size == 2 + assert len(hardware_info.gpios[0].state_interfaces) == 2 + assert hardware_info.gpios[0].state_interfaces[0].name == "analog_input" + assert hardware_info.gpios[0].state_interfaces[0].data_type == "double" + assert hardware_info.gpios[0].state_interfaces[0].size == 3 + assert hardware_info.gpios[0].state_interfaces[1].name == "image" + assert hardware_info.gpios[0].state_interfaces[1].data_type == "cv::Mat" + assert hardware_info.gpios[0].state_interfaces[1].size == 1 + + +def test_successfully_parse_parameter_empty(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.valid_urdf_ros2_control_parameter_empty + + ros2_control_test_assets.urdf_tail + ) + + control_hardware = parse_control_resources_from_urdf(urdf_to_test) + assert len(control_hardware) == 1 + hardware_info = control_hardware[0] + + assert hardware_info.name == "2DOF_System_Robot_Position_Only" + assert hardware_info.type == "system" + assert ( + getattr(hardware_info, hardware_plugin_name) + == "ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only" + ) + + assert len(hardware_info.joints) == 1 + + assert hardware_info.joints[0].name == "joint1" + assert hardware_info.joints[0].type == "joint" + assert hardware_info.joints[0].command_interfaces[0].name == "position" + + assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "" + assert hardware_info.hardware_parameters["example_param_read_for_sec"] == "2" + + +def test_negative_size_throws_error(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.invalid_urdf2_ros2_control_illegal_size + + ros2_control_test_assets.urdf_tail + ) + + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(urdf_to_test) + + +def test_noninteger_size_throws_error(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.invalid_urdf2_ros2_control_illegal_size2 + + ros2_control_test_assets.urdf_tail + ) + + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(urdf_to_test) + + +def test_transmission_and_component_joint_mismatch_throws_error(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.invalid_urdf2_hw_transmission_joint_mismatch + + ros2_control_test_assets.urdf_tail + ) + + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(urdf_to_test) + + +def test_transmission_given_too_many_joints_throws_error(): + urdf_to_test = ( + ros2_control_test_assets.urdf_head + + ros2_control_test_assets.invalid_urdf2_transmission_given_too_many_joints + + ros2_control_test_assets.urdf_tail + ) + + with pytest.raises(RuntimeError): + parse_control_resources_from_urdf(urdf_to_test) diff --git a/tests/hardware_interface/test_handle.py b/tests/hardware_interface/test_handle.py new file mode 100644 index 0000000..ac90160 --- /dev/null +++ b/tests/hardware_interface/test_handle.py @@ -0,0 +1,47 @@ +from ros2_control_py.hardware_interface import ( + CommandInterface, + StateInterface, + FloatRef, +) +import pytest + +JOINT_NAME = "joint_1" +FOO_INTERFACE = "FooInterface" + + +def test_command_interface(): + value = FloatRef(1.337) + interface = CommandInterface(JOINT_NAME, FOO_INTERFACE, value) + assert interface.get_value() == value.value() + interface.set_value(0.0) + assert interface.get_value() == 0.0 + + +def test_state_interface(): + value = FloatRef(1.337) + interface = StateInterface(JOINT_NAME, FOO_INTERFACE, value) + assert interface.get_value() == value.value() + assert not hasattr(interface, "set_value") + + +def test_name_getters_work(): + handle = StateInterface(JOINT_NAME, FOO_INTERFACE) + assert handle.get_name() == JOINT_NAME + "/" + FOO_INTERFACE + assert handle.get_interface_name() == FOO_INTERFACE + assert handle.get_prefix_name() == JOINT_NAME + + +def test_value_methods_throw_for_nullptr(): + handle = CommandInterface(JOINT_NAME, FOO_INTERFACE) + with pytest.raises(RuntimeError): + handle.get_value() + with pytest.raises(RuntimeError): + handle.set_value(0.0) + + +def test_value_methods_work_on_non_nullptr(): + value = FloatRef(1.337) + handle = CommandInterface(JOINT_NAME, FOO_INTERFACE, value) + assert handle.get_value() == value.value() + handle.set_value(0.0) + assert handle.get_value() == 0.0 diff --git a/tests/hardware_interface/test_inst_hardware.py b/tests/hardware_interface/test_inst_hardware.py new file mode 100644 index 0000000..24607eb --- /dev/null +++ b/tests/hardware_interface/test_inst_hardware.py @@ -0,0 +1,13 @@ +from ros2_control_py.hardware_interface import Actuator, Sensor, System + + +def test_build_actuator(): + actuator = Actuator() + + +def test_build_sensor(): + sensor = Sensor() + + +def test_build_actuator(): + system = System() diff --git a/tests/test_sanity.py b/tests/test_sanity.py new file mode 100644 index 0000000..02eb7ab --- /dev/null +++ b/tests/test_sanity.py @@ -0,0 +1,5 @@ +import ros2_control_py as rc + + +def test_sanity(): + assert 1 + 1 == 2 diff --git a/tests/utils.py b/tests/utils.py new file mode 100644 index 0000000..275a590 --- /dev/null +++ b/tests/utils.py @@ -0,0 +1,11 @@ +from ros2_control_py.hardware_interface import HardwareInfo +from os import getenv + +# Humble / Rolling name diff +ROS_DISTRO = getenv("ROS_DISTRO") +assert ROS_DISTRO in ("humble", "rolling") +hardware_plugin_name = ( + "hardware_class_type" + if hasattr(HardwareInfo, "hardware_class_type") + else "hardware_plugin_name" +) From 77524a6475c82c33c7f6ed8e8c9effa49fd2000e Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 18 Oct 2023 13:52:05 +0200 Subject: [PATCH 24/42] fix: new include struct on rolling --- CMakeLists.txt | 2 +- src/ros2_control_py_builder/main.cpp | 24 +++++++++++++++--------- 2 files changed, 16 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ccc943a..51a3e0a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,7 +45,7 @@ target_link_libraries(${PROJECT_NAME}_builder ) add_custom_command( OUTPUT ${_ros2_py_src} - COMMAND ${PROJECT_NAME}_builder "${CMAKE_CURRENT_BINARY_DIR}" "/opt/ros/$ENV{ROS_DISTRO}/include" ${_ros2_py_modules} + COMMAND ${PROJECT_NAME}_builder "${CMAKE_CURRENT_BINARY_DIR}" "/opt/ros/$ENV{ROS_DISTRO}/include" "$ENV{ROS_DISTRO}" ${_ros2_py_modules} DEPENDS ${PROJECT_NAME}_builder ) diff --git a/src/ros2_control_py_builder/main.cpp b/src/ros2_control_py_builder/main.cpp index 1c4624b..82e4556 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/src/ros2_control_py_builder/main.cpp @@ -15,21 +15,27 @@ void init_overloads(Module& mod); void init_header_order(Module& mod); int main(int argc, char** argv) { - ASSERT(argc > 3, - "Invalid number of command line arguments, expected at least 3 got " + ASSERT(argc > 4, + "Invalid number of command line arguments, expected at least 4 got " << argc - 1); - fs::path dst_dir = argv[1]; - fs::path inc_dir = argv[2]; - fs::path src_dir = dst_dir / "src"; + const fs::path dst_dir = argv[1]; + const fs::path inc_dir = argv[2]; + const std::string ros_distro = argv[3]; + const fs::path src_dir = dst_dir / "src"; fs::create_directories(src_dir); ASSERT_DIR(src_dir); ASSERT_DIR(inc_dir); + ASSERT(ros_distro == "humble" || ros_distro == "rolling", + "Unsupported ros distro " << ros_distro); + + const bool long_includes = ros_distro != "humble"; std::vector modules; - for (int i = 3; i < argc; ++i) - modules.emplace_back(inc_dir, src_dir, argv[i]); + for (int i = 4; i < argc; ++i) + modules.emplace_back(long_includes ? inc_dir / argv[i] : inc_dir, src_dir, + argv[i]); for (const Module& mod : modules) { fs::create_directories(mod.src_dir); @@ -91,8 +97,8 @@ void init_py_utils(Module& mod) { auto lni = std::make_shared(*mod.py_utils, "rclcpp_lifecycle::node_interfaces", "LifecycleNodeInterface", "", nullptr); - auto names = {"on_configure", "on_cleanup", "on_shutdown", - "on_activate", "on_deactivate", "on_error"}; + const auto names = {"on_configure", "on_cleanup", "on_shutdown", + "on_activate", "on_deactivate", "on_error"}; for (const auto& name : names) lni->membs.emplace_back( new Memb{name, From 420598191bc0993e6bf250d1c82a020bb0c8322e Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 18 Oct 2023 14:05:19 +0200 Subject: [PATCH 25/42] fix: removed locale test && joint_limits_interface dep --- package.xml | 1 - .../test_component_parser.py | 30 +------------------ 2 files changed, 1 insertion(+), 30 deletions(-) diff --git a/package.xml b/package.xml index 887108e..cefb5e8 100644 --- a/package.xml +++ b/package.xml @@ -28,7 +28,6 @@ controller_manager_msgs hardware_interface joint_limits - joint_limits_interface transmission_interface ament_cmake_pytest diff --git a/tests/hardware_interface/test_component_parser.py b/tests/hardware_interface/test_component_parser.py index 839ca29..0a5f579 100644 --- a/tests/hardware_interface/test_component_parser.py +++ b/tests/hardware_interface/test_component_parser.py @@ -6,9 +6,8 @@ ) from ros2_control_py import ros2_control_test_assets import pytest -from locale import setlocale from math import isclose, pi -from tests.utils import ROS_DISTRO, hardware_plugin_name +from tests.utils import hardware_plugin_name def test_empty_string_throws_error(): @@ -522,33 +521,6 @@ def test_successfully_parse_valid_urdf_actuator_only(): assert transmission.parameters["additional_special_parameter"] == "1337" -def test_successfully_parse_locale_independent_double(): - if ROS_DISTRO == "humble": - return - # Set to locale with comma-separated decimals - setlocale(locale.LC_NUMERIC, ("fr_FR", "UTF-8")) - - urdf_to_test = ( - ros2_control_test_assets.urdf_head - + ros2_control_test_assets.valid_urdf_ros2_control_actuator_only - + ros2_control_test_assets.urdf_tail - ) - - control_hardware = parse_control_resources_from_urdf(urdf_to_test) - assert len(control_hardware) == 1 - hardware_info = control_hardware[0] - - assert hardware_info.hardware_parameters["example_param_write_for_sec"] == "1.13" - - assert len(hardware_info.transmissions) == 1 - transmission = hardware_info.transmissions[0] - assert len(transmission.joints) == 1 - joint = transmission.joints[0] - - # Test that we still parse doubles using dot notation - assert joint.mechanical_reduction == 325.949 - - def test_successfully_parse_valid_urdf_system_robot_with_gpio(): urdf_to_test = ( ros2_control_test_assets.urdf_head From 27055188c51348b484bb278c982d975faa845f6b Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 18 Oct 2023 16:28:32 +0200 Subject: [PATCH 26/42] feat: added doc & tests && ops on FloatRef --- doc/index.rst | 88 ++++++++++++++- src/ros2_control_py_builder/write.hpp | 59 ++++++++-- tests/__init__.py | 0 tests/hardware_interface/__init__.py | 0 .../test_component_parser.py | 8 +- tests/hardware_interface/test_float_ref.py | 103 ++++++++++++++++++ tests/hardware_interface/test_handle.py | 6 +- tests/utils.py | 11 -- 8 files changed, 246 insertions(+), 29 deletions(-) delete mode 100644 tests/__init__.py delete mode 100644 tests/hardware_interface/__init__.py create mode 100644 tests/hardware_interface/test_float_ref.py delete mode 100644 tests/utils.py diff --git a/doc/index.rst b/doc/index.rst index 9c00d6c..4718cdc 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,6 +1,90 @@ -=============== +############### ros2_control_py +############### + +Parsed Modules +============== + +* hardware_interface +* controller_interface +* ros2_control_test_assets + +Tested Features =============== -Nothing here for now... +hardware_interface +------------------ + +* parse_control_resources_from_urdf +* CommandInterface / CommandInterface (with FloatRef, see :ref:`_FloatRef`) +* Actuator / Sensor / System +* ActuatorInterface +* FloatRef + +New Interface +============= + +hardware_interface +------------------ + +.. _FloatRef: + +FloatRef +^^^^^^^^ + +FloatRef is an owning reference to a ``double``. +It decays into a ``double`` or a ``double*`` for interfaces that require it. +It's purpose is to be used with CommandInterface/CommandInterface. + +Warning: + | Although you can use assignment operators like +=, + | you **cannot** assign to a FloatRef. + | To do that see :ref:`_FloatRefProp` + +Usage: + +.. code:: python + + from ros2_control_py.hardware_interface import FloatRef, CommandInterface, HW_IF_VELOCITY + fr = FloatRef(5) + assert fr == 5 + fr += 3 + assert fr == 8 + fr.set_value(5 / 2) + assert fr == 2.5 + ci = CommandInterface("name", HW_IF_VELOCITY, fr) + assert ci.get_value() == 2.5 + fr.set_value(4) + assert ci.get_value() == 4 + ci.set_value(5) + assert fr == 5 + +.. _FloatRefProp: + +FloatRefProp +^^^^^^^^^^^^ + +FloatRefProp is a ``property`` like descriptor that handles instance wide FloatRef so that you can assign to them like normal floats. + +Usage: + +.. code:: python + + from ros2_control_py.hardware_interface import FloatRefProp, CommandInterface, HW_IF_VELOCITY + class Dummy: + fr = FloatRefProp(5) + def __init__(self): + self.ci = CommandInterface("name", HW_IF_VELOCITY, self.fr) + + d = Dummy() + assert d.fr == 5 + assert d.ci.get_value() == 5 + d.fr += 3 + assert d.fr == 8 + d.fr = 5 / 2 + assert d.fr == 2.5 + d.fr = 4 + assert d.ci.get_value() == 4 + d.ci.set_value(5) + assert d.fr == 5 diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index 652adb8..ff3a88a 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -228,9 +228,10 @@ class Ref { T& operator*() const { return *value_; } T* operator->() const { return value_.get(); } - T value() const { return *value_; } + T get_value() const { return *value_; } + Ref& set_value(double value) { *value_ = value; return *this; } - std::string str() const { + std::string repr() const { std::ostringstream oss; oss << *value_; return std::move(oss).str(); @@ -278,8 +279,12 @@ inline void init_)" if (header.name == mod.py_utils->name) { ofs << R"( py::class_>(m, "FloatRef") .def(py::init()) - .def("__repr__", &Ref::str) - .def("value", &Ref::value) + .def("__repr__", &Ref::repr) + .def("__float__", &Ref::get_value) + .def("get_value", &Ref::get_value) + .def("set_value", &Ref::set_value) + .def("set_value", [](Ref& py_self, long long value) { return py_self.set_value(value); }) + .def("set_value", [](Ref& py_self, const Ref& value) { return py_self.set_value(value); }) .def("__add__", [](const Ref& lhs, const Ref& rhs) { return *lhs + *rhs; }) .def("__add__", [](const Ref& lhs, double rhs) { return *lhs + rhs; }) .def("__add__", [](const Ref& lhs, long long rhs) { return *lhs + rhs; }) @@ -304,14 +309,44 @@ inline void init_)" .def("__imul__", [](Ref& lhs, const Ref& rhs) { *lhs *= *rhs; return lhs; }) .def("__imul__", [](Ref& lhs, double rhs) { *lhs *= rhs; return lhs; }) .def("__imul__", [](Ref& lhs, long long rhs) { *lhs *= rhs; return lhs; }) - .def("__div__", [](const Ref& lhs, const Ref& rhs) { return *lhs / *rhs; }) - .def("__div__", [](const Ref& lhs, double rhs) { return *lhs / rhs; }) - .def("__div__", [](const Ref& lhs, long long rhs) { return *lhs / rhs; }) - .def("__rdiv__", [](const Ref& rhs, double lhs) { return lhs / *rhs; }) - .def("__rdiv__", [](const Ref& rhs, long long lhs) { return lhs / *rhs; }) - .def("__idiv__", [](Ref& lhs, const Ref& rhs) { *lhs /= *rhs; return lhs; }) - .def("__idiv__", [](Ref& lhs, double rhs) { *lhs /= rhs; return lhs; }) - .def("__idiv__", [](Ref& lhs, long long rhs) { *lhs /= rhs; return lhs; }); + .def("__truediv__", [](const Ref& lhs, const Ref& rhs) { return *lhs / *rhs; }) + .def("__truediv__", [](const Ref& lhs, double rhs) { return *lhs / rhs; }) + .def("__truediv__", [](const Ref& lhs, long long rhs) { return *lhs / rhs; }) + .def("__rtruediv__", [](const Ref& rhs, double lhs) { return lhs / *rhs; }) + .def("__rtruediv__", [](const Ref& rhs, long long lhs) { return lhs / *rhs; }) + .def("__itruediv__", [](Ref& lhs, const Ref& rhs) { *lhs /= *rhs; return lhs; }) + .def("__itruediv__", [](Ref& lhs, double rhs) { *lhs /= rhs; return lhs; }) + .def("__itruediv__", [](Ref& lhs, long long rhs) { *lhs /= rhs; return lhs; }) + .def("__eq__", [](const Ref& lhs, const Ref& rhs) { return *lhs == *rhs; }) + .def("__eq__", [](const Ref& lhs, double rhs) { return *lhs == rhs; }) + .def("__eq__", [](const Ref& lhs, long long rhs) { return *lhs == rhs; }) + .def("__req__", [](const Ref& rhs, double lhs) { return lhs == *rhs; }) + .def("__req__", [](const Ref& rhs, long long lhs) { return lhs == *rhs; }) + .def("__ne__", [](const Ref& lhs, const Ref& rhs) { return *lhs != *rhs; }) + .def("__ne__", [](const Ref& lhs, double rhs) { return *lhs != rhs; }) + .def("__ne__", [](const Ref& lhs, long long rhs) { return *lhs != rhs; }) + .def("__rne__", [](const Ref& rhs, double lhs) { return lhs != *rhs; }) + .def("__rne__", [](const Ref& rhs, long long lhs) { return lhs != *rhs; }) + .def("__ge__", [](const Ref& lhs, const Ref& rhs) { return *lhs >= *rhs; }) + .def("__ge__", [](const Ref& lhs, double rhs) { return *lhs >= rhs; }) + .def("__ge__", [](const Ref& lhs, long long rhs) { return *lhs >= rhs; }) + .def("__rge__", [](const Ref& rhs, double lhs) { return lhs >= *rhs; }) + .def("__rge__", [](const Ref& rhs, long long lhs) { return lhs >= *rhs; }) + .def("__gt__", [](const Ref& lhs, const Ref& rhs) { return *lhs > *rhs; }) + .def("__gt__", [](const Ref& lhs, double rhs) { return *lhs > rhs; }) + .def("__gt__", [](const Ref& lhs, long long rhs) { return *lhs > rhs; }) + .def("__rgt__", [](const Ref& rhs, double lhs) { return lhs > *rhs; }) + .def("__rgt__", [](const Ref& rhs, long long lhs) { return lhs > *rhs; }) + .def("__le__", [](const Ref& lhs, const Ref& rhs) { return *lhs <= *rhs; }) + .def("__le__", [](const Ref& lhs, double rhs) { return *lhs <= rhs; }) + .def("__le__", [](const Ref& lhs, long long rhs) { return *lhs <= rhs; }) + .def("__rle__", [](const Ref& rhs, double lhs) { return lhs <= *rhs; }) + .def("__rle__", [](const Ref& rhs, long long lhs) { return lhs <= *rhs; }) + .def("__lt__", [](const Ref& lhs, const Ref& rhs) { return *lhs < *rhs; }) + .def("__lt__", [](const Ref& lhs, double rhs) { return *lhs < rhs; }) + .def("__lt__", [](const Ref& lhs, long long rhs) { return *lhs < rhs; }) + .def("__rlt__", [](const Ref& rhs, double lhs) { return lhs < *rhs; }) + .def("__rlt__", [](const Ref& rhs, long long lhs) { return lhs < *rhs; }); py::class_>(m, "FloatRefProp") .def(py::init()) diff --git a/tests/__init__.py b/tests/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/tests/hardware_interface/__init__.py b/tests/hardware_interface/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/tests/hardware_interface/test_component_parser.py b/tests/hardware_interface/test_component_parser.py index 0a5f579..5d9b5f0 100644 --- a/tests/hardware_interface/test_component_parser.py +++ b/tests/hardware_interface/test_component_parser.py @@ -3,11 +3,17 @@ HW_IF_POSITION, HW_IF_VELOCITY, parse_control_resources_from_urdf, + HardwareInfo, ) from ros2_control_py import ros2_control_test_assets import pytest from math import isclose, pi -from tests.utils import hardware_plugin_name + +hardware_plugin_name = ( + "hardware_class_type" + if hasattr(HardwareInfo, "hardware_class_type") + else "hardware_plugin_name" +) def test_empty_string_throws_error(): diff --git a/tests/hardware_interface/test_float_ref.py b/tests/hardware_interface/test_float_ref.py new file mode 100644 index 0000000..fad9850 --- /dev/null +++ b/tests/hardware_interface/test_float_ref.py @@ -0,0 +1,103 @@ +from ros2_control_py.hardware_interface import FloatRef + + +def assert_cmp(a, b, ta, tb): + fa, fb = ta(a), tb(b) + assert (a == b) == (fa == fb) + assert (a != b) == (fa != fb) + assert (a >= b) == (fa >= fb) + assert (a > b) == (fa > fb) + assert (a <= b) == (fa <= fb) + assert (a < b) == (fa < fb) + + +def assert_arith(a, b, ta, tb): + fa, fb = ta(a), tb(b) + r = fa + fb + assert type(r) is float + assert (a + b) == r + r = fa - fb + assert type(r) is float + assert (a - b) == r + r = fa * fb + assert type(r) is float + assert (a * b) == r + if b == 0: + return + r = fa / fb + assert type(r) is float + assert (a / b) == r + + +def assert_self_arith(a, b, tb): + fb = tb(b) + r = FloatRef(a) + r += fb + assert type(r) is FloatRef + assert (a + b) == r.get_value() + r = FloatRef(a) + r -= fb + assert type(r) is FloatRef + assert (a - b) == r.get_value() + r = FloatRef(a) + r *= fb + assert type(r) is FloatRef + assert (a * b) == r.get_value() + if b == 0: + return + r = FloatRef(a) + r /= fb + assert type(r) is FloatRef + assert (a / b) == r.get_value() + + +def assert_cmpi(a, b, ta, tb): + assert_cmp(int(a), int(b), ta, tb) + + +def assert_arithi(a, b, ta, tb): + assert_arith(int(a), int(b), ta, tb) + + +def assert_self_arithi(a, b, tb): + assert_self_arith(int(a), int(b), tb) + + +def assert_cmps(a, b): + assert_cmp(a, b, FloatRef, FloatRef) + assert_cmp(a, b, FloatRef, float) + assert_cmp(a, b, float, FloatRef) + assert_cmpi(a, b, FloatRef, int) + assert_cmpi(a, b, int, FloatRef) + + +def assert_ariths(a, b): + assert_arith(a, b, FloatRef, FloatRef) + assert_arith(a, b, FloatRef, float) + assert_arith(a, b, float, FloatRef) + assert_arithi(a, b, FloatRef, int) + assert_arithi(a, b, int, FloatRef) + + +def assert_self_ariths(a, b): + assert_self_arith(a, b, FloatRef) + assert_self_arith(a, b, float) + assert_self_arithi(a, b, int) + + +def test_cmp(): + for i in range(-10, 11): + for j in range(-10, 11): + assert_cmps(i * 1.5, j / 2) + + +def test_arith(): + for i in range(-10, 11): + for j in range(-10, 11): + assert_ariths(i * 1.5, j / 2) + + +def test_self_arith(): + for i in range(-10, 11): + for j in range(-10, 11): + assert_self_ariths(i * 1.5, j / 2) diff --git a/tests/hardware_interface/test_handle.py b/tests/hardware_interface/test_handle.py index ac90160..0ae0b25 100644 --- a/tests/hardware_interface/test_handle.py +++ b/tests/hardware_interface/test_handle.py @@ -12,7 +12,7 @@ def test_command_interface(): value = FloatRef(1.337) interface = CommandInterface(JOINT_NAME, FOO_INTERFACE, value) - assert interface.get_value() == value.value() + assert interface.get_value() == value.get_value() interface.set_value(0.0) assert interface.get_value() == 0.0 @@ -20,7 +20,7 @@ def test_command_interface(): def test_state_interface(): value = FloatRef(1.337) interface = StateInterface(JOINT_NAME, FOO_INTERFACE, value) - assert interface.get_value() == value.value() + assert interface.get_value() == value.get_value() assert not hasattr(interface, "set_value") @@ -42,6 +42,6 @@ def test_value_methods_throw_for_nullptr(): def test_value_methods_work_on_non_nullptr(): value = FloatRef(1.337) handle = CommandInterface(JOINT_NAME, FOO_INTERFACE, value) - assert handle.get_value() == value.value() + assert handle.get_value() == value.get_value() handle.set_value(0.0) assert handle.get_value() == 0.0 diff --git a/tests/utils.py b/tests/utils.py deleted file mode 100644 index 275a590..0000000 --- a/tests/utils.py +++ /dev/null @@ -1,11 +0,0 @@ -from ros2_control_py.hardware_interface import HardwareInfo -from os import getenv - -# Humble / Rolling name diff -ROS_DISTRO = getenv("ROS_DISTRO") -assert ROS_DISTRO in ("humble", "rolling") -hardware_plugin_name = ( - "hardware_class_type" - if hasattr(HardwareInfo, "hardware_class_type") - else "hardware_plugin_name" -) From ff0a7b30d7da851211edaf2d043bfa18ff8d6ddc Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 18 Oct 2023 16:38:33 +0200 Subject: [PATCH 27/42] fix: test div by zero --- tests/hardware_interface/test_float_ref.py | 25 ++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/tests/hardware_interface/test_float_ref.py b/tests/hardware_interface/test_float_ref.py index fad9850..7a46372 100644 --- a/tests/hardware_interface/test_float_ref.py +++ b/tests/hardware_interface/test_float_ref.py @@ -1,4 +1,21 @@ from ros2_control_py.hardware_interface import FloatRef +from math import nan, inf, isnan + + +def cpp_div(a, b): + if b != 0: + return a / b + if a == 0: + return nan + if a < 0: + return -inf + if a > 0: + return inf + return nan + + +def assert_same_float(a, b): + assert a == b or (isnan(a) and isnan(b)) def assert_cmp(a, b, ta, tb): @@ -22,11 +39,9 @@ def assert_arith(a, b, ta, tb): r = fa * fb assert type(r) is float assert (a * b) == r - if b == 0: - return r = fa / fb assert type(r) is float - assert (a / b) == r + assert_same_float(cpp_div(a, b), r) def assert_self_arith(a, b, tb): @@ -43,12 +58,10 @@ def assert_self_arith(a, b, tb): r *= fb assert type(r) is FloatRef assert (a * b) == r.get_value() - if b == 0: - return r = FloatRef(a) r /= fb assert type(r) is FloatRef - assert (a / b) == r.get_value() + assert_same_float(cpp_div(a, b), r.get_value()) def assert_cmpi(a, b, ta, tb): From 422bc7573210da4cfd66cd29483c25ebc7197b46 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 18 Oct 2023 16:44:34 +0200 Subject: [PATCH 28/42] feat(doc): added mention to set_value --- doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index 4718cdc..caab736 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -40,7 +40,7 @@ It's purpose is to be used with CommandInterface/CommandInterface. Warning: | Although you can use assignment operators like +=, | you **cannot** assign to a FloatRef. - | To do that see :ref:`_FloatRefProp` + | To do that see :ref:`_FloatRefProp` of use set_value. Usage: From dfd03be236ab61bd64216e9d7fd43c76a9617c9b Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Wed, 18 Oct 2023 16:49:37 +0200 Subject: [PATCH 29/42] fix(doc): typo --- doc/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/index.rst b/doc/index.rst index caab736..6c71675 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -17,7 +17,7 @@ hardware_interface ------------------ * parse_control_resources_from_urdf -* CommandInterface / CommandInterface (with FloatRef, see :ref:`_FloatRef`) +* StateInterface / CommandInterface (with FloatRef, see :ref:`_FloatRef`) * Actuator / Sensor / System * ActuatorInterface * FloatRef From e780c83ed0f30a8bafe3d0c6128cb0d3b6989853 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Thu, 19 Oct 2023 09:26:37 +0200 Subject: [PATCH 30/42] fix(doc): typos and refs --- doc/index.rst | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 6c71675..702de4e 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -17,10 +17,10 @@ hardware_interface ------------------ * parse_control_resources_from_urdf -* StateInterface / CommandInterface (with FloatRef, see :ref:`_FloatRef`) +* StateInterface / CommandInterface (see FloatRef_ and FloatRefProp_) * Actuator / Sensor / System * ActuatorInterface -* FloatRef +* FloatRef_ New Interface ============= @@ -35,12 +35,12 @@ FloatRef FloatRef is an owning reference to a ``double``. It decays into a ``double`` or a ``double*`` for interfaces that require it. -It's purpose is to be used with CommandInterface/CommandInterface. +It's purpose is to be used with StateInterface/CommandInterface. Warning: | Although you can use assignment operators like +=, | you **cannot** assign to a FloatRef. - | To do that see :ref:`_FloatRefProp` of use set_value. + | To do that see FloatRefProp_ of use set_value. Usage: From 8cdebc313f65549a0ebf6c616ebf25b461a4c73b Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Thu, 19 Oct 2023 11:04:05 +0200 Subject: [PATCH 31/42] feat: added assignment operator for FloatRef && added tests for ActuatorInterface & SensorInterface & SystemInterface --- doc/index.rst | 20 +- src/ros2_control_py_builder/write.hpp | 6 + .../test_component_interface.py | 690 +++++++++++++++++- tests/hardware_interface/test_float_ref.py | 4 + 4 files changed, 685 insertions(+), 35 deletions(-) diff --git a/doc/index.rst b/doc/index.rst index 702de4e..894d57b 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -19,7 +19,7 @@ hardware_interface * parse_control_resources_from_urdf * StateInterface / CommandInterface (see FloatRef_ and FloatRefProp_) * Actuator / Sensor / System -* ActuatorInterface +* ActuatorInterface / SensorInterface / SystemInterface * FloatRef_ New Interface @@ -33,14 +33,14 @@ hardware_interface FloatRef ^^^^^^^^ -FloatRef is an owning reference to a ``double``. -It decays into a ``double`` or a ``double*`` for interfaces that require it. -It's purpose is to be used with StateInterface/CommandInterface. +| FloatRef is an owning reference to a ``double`` that behaves as a float-like object in Python. +| In C++ it decays into a ``double`` or a ``double*`` for interfaces that require it. +| It's purpose is to be used with StateInterface/CommandInterface. Warning: | Although you can use assignment operators like +=, - | you **cannot** assign to a FloatRef. - | To do that see FloatRefProp_ of use set_value. + | you **cannot** assign to a FloatRef with =. + | To do that see FloatRefProp_ of use @ / @= / set_value. Usage: @@ -53,9 +53,13 @@ Usage: assert fr == 8 fr.set_value(5 / 2) assert fr == 2.5 + fr @ 8 + assert fr == 8 + fr @= 7 + assert fr == 7 ci = CommandInterface("name", HW_IF_VELOCITY, fr) - assert ci.get_value() == 2.5 - fr.set_value(4) + assert ci.get_value() == 7 + fr @= 4 assert ci.get_value() == 4 ci.set_value(5) assert fr == 5 diff --git a/src/ros2_control_py_builder/write.hpp b/src/ros2_control_py_builder/write.hpp index ff3a88a..aecd520 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/src/ros2_control_py_builder/write.hpp @@ -285,6 +285,12 @@ inline void init_)" .def("set_value", &Ref::set_value) .def("set_value", [](Ref& py_self, long long value) { return py_self.set_value(value); }) .def("set_value", [](Ref& py_self, const Ref& value) { return py_self.set_value(value); }) + .def("__matmul__", &Ref::set_value) + .def("__matmul__", [](Ref& lhs, const Ref& rhs) { return lhs.set_value(rhs); }) + .def("__matmul__", [](Ref& lhs, long long rhs) { return lhs.set_value(rhs); }) + .def("__imatmul__", &Ref::set_value) + .def("__imatmul__", [](Ref& lhs, const Ref& rhs) { return lhs.set_value(rhs); }) + .def("__imatmul__", [](Ref& lhs, long long rhs) { return lhs.set_value(rhs); }) .def("__add__", [](const Ref& lhs, const Ref& rhs) { return *lhs + *rhs; }) .def("__add__", [](const Ref& lhs, double rhs) { return *lhs + rhs; }) .def("__add__", [](const Ref& lhs, long long rhs) { return *lhs + rhs; }) diff --git a/tests/hardware_interface/test_component_interface.py b/tests/hardware_interface/test_component_interface.py index 908e115..8786a0f 100644 --- a/tests/hardware_interface/test_component_interface.py +++ b/tests/hardware_interface/test_component_interface.py @@ -1,6 +1,10 @@ from ros2_control_py.hardware_interface import ( Actuator, + Sensor, + System, ActuatorInterface, + SensorInterface, + SystemInterface, CommandInterface, StateInterface, HardwareInfo, @@ -11,10 +15,11 @@ INACTIVE, FINALIZED, return_type, - FloatRefProp, CallbackReturn, Time, Duration, + FloatRefProp, + FloatRef, VectorString, ) from lifecycle_msgs.msg import State @@ -30,16 +35,16 @@ class DummyActuator(ActuatorInterface): velocity_state_ = FloatRefProp(nan) velocity_command_ = FloatRefProp(0.0) - def __init__(self): + def __init__(self) -> None: super().__init__() self.read_calls_ = 0 self.write_calls_ = 0 self.recoverable_error_happened_ = False - def on_init(self, info): + def on_init(self, info: HardwareInfo) -> CallbackReturn: return CallbackReturn.SUCCESS - def on_configure(self, previous_state): + def on_configure(self, previous_state: State) -> CallbackReturn: self.position_state_ = 0.0 self.velocity_state_ = 0.0 if self.recoverable_error_happened_: @@ -48,35 +53,27 @@ def on_configure(self, previous_state): self.write_calls_ = 0 return CallbackReturn.SUCCESS - def export_state_interfaces(self): - state_interfaces = [] - state_interfaces.append( - StateInterface("joint1", HW_IF_POSITION, self.position_state_) - ) - state_interfaces.append( - StateInterface("joint1", HW_IF_VELOCITY, self.velocity_state_) - ) - return state_interfaces - - def export_command_interfaces(self): + def export_state_interfaces(self) -> list[StateInterface]: + return [ + StateInterface("joint1", HW_IF_POSITION, self.position_state_), + StateInterface("joint1", HW_IF_VELOCITY, self.velocity_state_), + ] + + def export_command_interfaces(self) -> list[CommandInterface]: # We can command in velocity - command_interfaces = [] - command_interfaces.append( - CommandInterface("joint1", HW_IF_VELOCITY, self.velocity_command_) - ) - return command_interfaces + return [CommandInterface("joint1", HW_IF_VELOCITY, self.velocity_command_)] - def get_name(self): + def get_name(self) -> str: return "DummyActuator" - def read(self, time, period): + def read(self, time: Time, period: Duration) -> return_type: self.read_calls_ += 1 if self.read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS: return return_type.ERROR # no-op, state is getting propagated within write. return return_type.OK - def write(self, time, period): + def write(self, time: Time, period: Duration) -> return_type: self.write_calls_ += 1 if self.write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS: return return_type.ERROR @@ -84,11 +81,54 @@ def write(self, time, period): self.velocity_state_ = self.velocity_command_ return return_type.OK - def on_shutdown(self, previous_state): + def on_shutdown(self, previous_state: State) -> CallbackReturn: self.velocity_state_ = 0 return CallbackReturn.SUCCESS - def on_error(self, previous_state): + def on_error(self, previous_state: State) -> CallbackReturn: + if not self.recoverable_error_happened_: + self.recoverable_error_happened_ = True + return CallbackReturn.SUCCESS + else: + return CallbackReturn.ERROR + return CallbackReturn.FAILURE + + +class DummySensor(SensorInterface): + voltage_level_ = FloatRefProp(nan) + + def __init__(self) -> None: + super().__init__() + self.voltage_level_hw_value_ = 0x666 + # Helper variables to initiate error on read + self.read_calls_ = 0 + self.recoverable_error_happened_ = False + + def on_init(self, info: HardwareInfo) -> CallbackReturn: + # We hardcode the info + return CallbackReturn.SUCCESS + + def on_configure(self, previous_state: State) -> CallbackReturn: + self.voltage_level_ = 0.0 + self.read_calls_ = 0 + return CallbackReturn.SUCCESS + + def export_state_interfaces(self) -> list[StateInterface]: + # We can read some voltage level + return [StateInterface("joint1", "voltage", self.voltage_level_)] + + def get_name(self) -> str: + return "DummySensor" + + def read(self, time: Time, period: Duration) -> return_type: + self.read_calls_ += 1 + if self.read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS: + return return_type.ERROR + # no-op, static value + self.voltage_level_ = self.voltage_level_hw_value_ + return return_type.OK + + def on_error(self, previous_state: State) -> CallbackReturn: if not self.recoverable_error_happened_: self.recoverable_error_happened_ = True return CallbackReturn.SUCCESS @@ -97,10 +137,139 @@ def on_error(self, previous_state): return CallbackReturn.FAILURE +class DummySystem(SystemInterface): + def __init__(self): + super().__init__() + self.position_state_ = [FloatRef(nan), FloatRef(nan), FloatRef(nan)] + self.velocity_state_ = [FloatRef(nan), FloatRef(nan), FloatRef(nan)] + self.velocity_command_ = [FloatRef(0), FloatRef(0), FloatRef(0)] + # Helper variables to initiate error on read + self.read_calls_ = 0 + self.write_calls_ = 0 + self.recoverable_error_happened_ = False + + def on_init(self, info: HardwareInfo) -> CallbackReturn: + # We hardcode the info + return CallbackReturn.SUCCESS + + def on_configure(self, previous_state: State) -> CallbackReturn: + for p, v in zip(self.position_state_, self.velocity_state_): + p @= 0 + v @= 0 + # reset command only if error is initiated + if self.recoverable_error_happened_: + for v in self.velocity_command_: + v @= 0 + self.read_calls_ = 0 + self.write_calls_ = 0 + return CallbackReturn.SUCCESS + + def export_state_interfaces(self) -> list[StateInterface]: + # We can read a position and a velocity + return [ + StateInterface("joint1", HW_IF_POSITION, self.position_state_[0]), + StateInterface("joint1", HW_IF_VELOCITY, self.velocity_state_[0]), + StateInterface("joint2", HW_IF_POSITION, self.position_state_[1]), + StateInterface("joint2", HW_IF_VELOCITY, self.velocity_state_[1]), + StateInterface("joint3", HW_IF_POSITION, self.position_state_[2]), + StateInterface("joint3", HW_IF_VELOCITY, self.velocity_state_[2]), + ] + + def export_command_interfaces(self) -> list[CommandInterface]: + # We can command in velocity + return [ + CommandInterface("joint1", HW_IF_VELOCITY, self.velocity_command_[0]), + CommandInterface("joint2", HW_IF_VELOCITY, self.velocity_command_[1]), + CommandInterface("joint3", HW_IF_VELOCITY, self.velocity_command_[2]), + ] + + def get_name(self) -> str: + return "DummySystem" + + def read(self, time: Time, period: Duration) -> return_type: + self.read_calls_ += 1 + if self.read_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS: + return return_type.ERROR + # no-op, state is getting propagated within write. + return return_type.OK + + def write(self, time: Time, period: Duration) -> return_type: + self.write_calls_ += 1 + if self.write_calls_ == TRIGGER_READ_WRITE_ERROR_CALLS: + return return_type.ERROR + for p, v in zip(self.position_state_, self.velocity_state_): + p += self.velocity_command_[0] + v @= self.velocity_command_[0] + return return_type.OK + + def on_shutdown(self, previous_state: State) -> CallbackReturn: + for v in self.velocity_state_: + v @= 0 + return CallbackReturn.SUCCESS + + def on_error(self, previous_state: State) -> CallbackReturn: + if not self.recoverable_error_happened_: + self.recoverable_error_happened_ = True + return CallbackReturn.SUCCESS + else: + return CallbackReturn.ERROR + return CallbackReturn.FAILURE + + +class DummySystemPreparePerform(SystemInterface): + def __init__(self): + super().__init__() + + # Override the pure virtual functions with default behavior + def on_init(self, info: HardwareInfo) -> CallbackReturn: + # We hardcode the info + return CallbackReturn.SUCCESS + + def export_state_interfaces(self) -> list[StateInterface]: + return [] + + def export_command_interfaces(self) -> list[CommandInterface]: + return [] + + def get_name(self) -> str: + return "DummySystemPreparePerform" + + def read(self, time: Time, period: Duration) -> return_type: + return return_type.OK + + def write(self, time: Time, period: Duration) -> return_type: + return return_type.OK + + # Custom prepare/perform functions + def prepare_command_mode_switch( + self, start_interfaces: VectorString, stop_interfaces: VectorString + ) -> return_type: + # Criteria to test against + if len(start_interfaces) != 1: + return return_type.ERROR + if len(stop_interfaces) != 2: + return return_type.ERROR + return return_type.OK + + def perform_command_mode_switch( + self, start_interfaces: VectorString, stop_interfaces: VectorString + ) -> return_type: + # Criteria to test against + if len(start_interfaces) != 1: + return return_type.ERROR + if len(stop_interfaces) != 2: + return return_type.ERROR + return return_type.OK + + def ASSERT_TRUE(a): assert a +def EXPECT_TRUE(a): + ASSERT_TRUE(a) + + def ASSERT_EQ(a, b): assert a == b @@ -110,8 +279,7 @@ def EXPECT_EQ(a, b): def test_dummy_actuator(): - dummy_actuator_hw = DummyActuator() - actuator_hw = Actuator(dummy_actuator_hw) + actuator_hw = Actuator(DummyActuator()) mock_hw_info = HardwareInfo() state = actuator_hw.initialize(mock_hw_info) @@ -199,3 +367,471 @@ def test_dummy_actuator(): return_type.OK, actuator_hw.perform_command_mode_switch(VectorString([""]), VectorString([""])), ) + + +def test_dummy_sensor(): + sensor_hw = Sensor(DummySensor()) + + mock_hw_info = HardwareInfo() + state = sensor_hw.initialize(mock_hw_info) + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + state_interfaces = sensor_hw.export_state_interfaces() + ASSERT_EQ(1, len(state_interfaces)) + EXPECT_EQ("joint1/voltage", state_interfaces[0].get_name()) + EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()) + EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()) + EXPECT_TRUE(isnan(state_interfaces[0].get_value())) + + # Not updated because is is UNCONFIGURED + sensor_hw.read(TIME, PERIOD) + EXPECT_TRUE(isnan(state_interfaces[0].get_value())) + + # Updated because is is INACTIVE + state = sensor_hw.configure() + EXPECT_EQ(State.PRIMARY_STATE_INACTIVE, state.id()) + EXPECT_EQ(INACTIVE, state.label()) + EXPECT_EQ(0, state_interfaces[0].get_value()) + + # It can read now + sensor_hw.read(TIME, PERIOD) + EXPECT_EQ(0x666, state_interfaces[0].get_value()) + + +def test_dummy_system(): + system_hw = System(DummySystem()) + + mock_hw_info = HardwareInfo() + state = system_hw.initialize(mock_hw_info) + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + state_interfaces = system_hw.export_state_interfaces() + ASSERT_EQ(6, len(state_interfaces)) + EXPECT_EQ("joint1/position", state_interfaces[0].get_name()) + EXPECT_EQ(HW_IF_POSITION, state_interfaces[0].get_interface_name()) + EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()) + EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()) + EXPECT_EQ(HW_IF_VELOCITY, state_interfaces[1].get_interface_name()) + EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()) + EXPECT_EQ("joint2/position", state_interfaces[2].get_name()) + EXPECT_EQ(HW_IF_POSITION, state_interfaces[2].get_interface_name()) + EXPECT_EQ("joint2", state_interfaces[2].get_prefix_name()) + EXPECT_EQ("joint2/velocity", state_interfaces[3].get_name()) + EXPECT_EQ(HW_IF_VELOCITY, state_interfaces[3].get_interface_name()) + EXPECT_EQ("joint2", state_interfaces[3].get_prefix_name()) + EXPECT_EQ("joint3/position", state_interfaces[4].get_name()) + EXPECT_EQ(HW_IF_POSITION, state_interfaces[4].get_interface_name()) + EXPECT_EQ("joint3", state_interfaces[4].get_prefix_name()) + EXPECT_EQ("joint3/velocity", state_interfaces[5].get_name()) + EXPECT_EQ(HW_IF_VELOCITY, state_interfaces[5].get_interface_name()) + EXPECT_EQ("joint3", state_interfaces[5].get_prefix_name()) + + command_interfaces = system_hw.export_command_interfaces() + ASSERT_EQ(3, len(command_interfaces)) + EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()) + EXPECT_EQ(HW_IF_VELOCITY, command_interfaces[0].get_interface_name()) + EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()) + EXPECT_EQ("joint2/velocity", command_interfaces[1].get_name()) + EXPECT_EQ(HW_IF_VELOCITY, command_interfaces[1].get_interface_name()) + EXPECT_EQ("joint2", command_interfaces[1].get_prefix_name()) + EXPECT_EQ("joint3/velocity", command_interfaces[2].get_name()) + EXPECT_EQ(HW_IF_VELOCITY, command_interfaces[2].get_interface_name()) + EXPECT_EQ("joint3", command_interfaces[2].get_prefix_name()) + + velocity_value = 1.0 + command_interfaces[0].set_value(velocity_value) # velocity + command_interfaces[1].set_value(velocity_value) # velocity + command_interfaces[2].set_value(velocity_value) # velocity + ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + + # Noting should change because it is UNCONFIGURED + for step in range(10): + ASSERT_EQ(return_type.ERROR, system_hw.read(TIME, PERIOD)) + + ASSERT_TRUE(isnan(state_interfaces[0].get_value())) # position value + ASSERT_TRUE(isnan(state_interfaces[1].get_value())) # velocity + ASSERT_TRUE(isnan(state_interfaces[2].get_value())) # position value + ASSERT_TRUE(isnan(state_interfaces[3].get_value())) # velocity + ASSERT_TRUE(isnan(state_interfaces[4].get_value())) # position value + ASSERT_TRUE(isnan(state_interfaces[5].get_value())) # velocity + + ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + + state = system_hw.configure() + EXPECT_EQ(State.PRIMARY_STATE_INACTIVE, state.id()) + EXPECT_EQ(INACTIVE, state.label()) + + # Read and Write are working because it is INACTIVE + for step in range(10): + ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) + + EXPECT_EQ( + step * velocity_value, state_interfaces[0].get_value() + ) # position value + EXPECT_EQ( + velocity_value if step else 0, state_interfaces[1].get_value() + ) # velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[2].get_value() + ) # position value + EXPECT_EQ( + velocity_value if step else 0, state_interfaces[3].get_value() + ) # velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[4].get_value() + ) # position value + EXPECT_EQ( + velocity_value if step else 0, state_interfaces[5].get_value() + ) # velocity + + ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + + state = system_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + # Read and Write are working because it is ACTIVE + for step in range(10): + ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) + + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[0].get_value() + ) # position value + EXPECT_EQ(velocity_value, state_interfaces[1].get_value()) # velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[2].get_value() + ) # position value + EXPECT_EQ(velocity_value, state_interfaces[3].get_value()) # velocity + EXPECT_EQ( + (10 + step) * velocity_value, state_interfaces[4].get_value() + ) # position value + EXPECT_EQ(velocity_value, state_interfaces[5].get_value()) # velocity + + ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + + state = system_hw.shutdown() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + # Noting should change because it is FINALIZED + for step in range(10): + ASSERT_EQ(return_type.ERROR, system_hw.read(TIME, PERIOD)) + + EXPECT_EQ( + 20 * velocity_value, state_interfaces[0].get_value() + ) # position value + EXPECT_EQ(0, state_interfaces[1].get_value()) # velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[2].get_value() + ) # position value + EXPECT_EQ(0, state_interfaces[3].get_value()) # velocity + EXPECT_EQ( + 20 * velocity_value, state_interfaces[4].get_value() + ) # position value + EXPECT_EQ(0, state_interfaces[5].get_value()) # velocity + + ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + + EXPECT_EQ( + return_type.OK, + system_hw.prepare_command_mode_switch(VectorString([""]), VectorString([""])), + ) + EXPECT_EQ( + return_type.OK, + system_hw.perform_command_mode_switch(VectorString([""]), VectorString([""])), + ) + + +def test_dummy_command_mode_system(): + system_hw = System(DummySystemPreparePerform()) + + mock_hw_info = HardwareInfo() + state = system_hw.initialize(mock_hw_info) + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + one_key = VectorString(["joint1/position"]) + two_keys = VectorString(["joint1/position", "joint1/velocity"]) + + # Only calls with (one_key, two_keys) should return OK + EXPECT_EQ( + return_type.ERROR, system_hw.prepare_command_mode_switch(one_key, one_key) + ) + EXPECT_EQ( + return_type.ERROR, system_hw.perform_command_mode_switch(one_key, one_key) + ) + EXPECT_EQ(return_type.OK, system_hw.prepare_command_mode_switch(one_key, two_keys)) + EXPECT_EQ(return_type.OK, system_hw.perform_command_mode_switch(one_key, two_keys)) + EXPECT_EQ( + return_type.ERROR, system_hw.prepare_command_mode_switch(two_keys, one_key) + ) + EXPECT_EQ( + return_type.ERROR, system_hw.perform_command_mode_switch(two_keys, one_key) + ) + + +def test_dummy_actuator_read_error_behavior(): + actuator_hw = Actuator(DummyActuator()) + + mock_hw_info = HardwareInfo() + state = actuator_hw.initialize(mock_hw_info) + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + state_interfaces = actuator_hw.export_state_interfaces() + command_interfaces = actuator_hw.export_command_interfaces() + state = actuator_hw.configure() + state = actuator_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + + # Initiate error on write (this is first time therefore recoverable) + for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, actuator_hw.read(TIME, PERIOD)) + + state = actuator_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + # activate again and expect reset values + state = actuator_hw.configure() + EXPECT_EQ(state_interfaces[0].get_value(), 0) + EXPECT_EQ(command_interfaces[0].get_value(), 0) + + state = actuator_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + + # Initiate error on write (this is the second time therefore unrecoverable) + for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, actuator_hw.read(TIME, PERIOD)) + + state = actuator_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + # can not change state anymore + state = actuator_hw.configure() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + +def test_dummy_actuator_write_error_behavior(): + actuator_hw = Actuator(DummyActuator()) + + mock_hw_info = HardwareInfo() + state = actuator_hw.initialize(mock_hw_info) + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + state_interfaces = actuator_hw.export_state_interfaces() + command_interfaces = actuator_hw.export_command_interfaces() + state = actuator_hw.configure() + state = actuator_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + + # Initiate error on write (this is first time therefore recoverable) + for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + + state = actuator_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + # activate again and expect reset values + state = actuator_hw.configure() + EXPECT_EQ(state_interfaces[0].get_value(), 0) + EXPECT_EQ(command_interfaces[0].get_value(), 0) + + state = actuator_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + + # Initiate error on write (this is the second time therefore unrecoverable) + for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + + state = actuator_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + # can not change state anymore + state = actuator_hw.configure() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + +def test_dummy_sensor_read_error_behavior(): + sensor_hw = Sensor(DummySensor()) + + mock_hw_info = HardwareInfo() + state = sensor_hw.initialize(mock_hw_info) + + state_interfaces = sensor_hw.export_state_interfaces() + # Updated because is is INACTIVE + state = sensor_hw.configure() + state = sensor_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + ASSERT_EQ(return_type.OK, sensor_hw.read(TIME, PERIOD)) + + # Initiate recoverable error - call read 99 times OK and on 100-time will return error + for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, sensor_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, sensor_hw.read(TIME, PERIOD)) + + state = sensor_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + # activate again and expect reset values + state = sensor_hw.configure() + EXPECT_EQ(state_interfaces[0].get_value(), 0) + + state = sensor_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + # Initiate unrecoverable error - call read 99 times OK and on 100-time will return error + for i in range(1, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, sensor_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, sensor_hw.read(TIME, PERIOD)) + + state = sensor_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + # can not change state anymore + state = sensor_hw.configure() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + +def test_dummy_system_read_error_behavior(): + system_hw = System(DummySystem()) + + mock_hw_info = HardwareInfo() + state = system_hw.initialize(mock_hw_info) + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + state_interfaces = system_hw.export_state_interfaces() + command_interfaces = system_hw.export_command_interfaces() + state = system_hw.configure() + state = system_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + + # Initiate error on write (this is first time therefore recoverable) + for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, system_hw.read(TIME, PERIOD)) + + state = system_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + # activate again and expect reset values + state = system_hw.configure() + for index in range(6): + EXPECT_EQ(state_interfaces[index].get_value(), 0) + for index in range(3): + EXPECT_EQ(command_interfaces[index].get_value(), 0) + state = system_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + + # Initiate error on write (this is the second time therefore unrecoverable) + for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, system_hw.read(TIME, PERIOD)) + + state = system_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + # can not change state anymore + state = system_hw.configure() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + +def test_dummy_system_write_error_behavior(): + system_hw = System(DummySystem()) + + mock_hw_info = HardwareInfo() + state = system_hw.initialize(mock_hw_info) + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + state_interfaces = system_hw.export_state_interfaces() + command_interfaces = system_hw.export_command_interfaces() + state = system_hw.configure() + state = system_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + + # Initiate error on write (this is first time therefore recoverable) + for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + + state = system_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) + EXPECT_EQ(UNCONFIGURED, state.label()) + + # activate again and expect reset values + state = system_hw.configure() + for index in range(6): + EXPECT_EQ(state_interfaces[index].get_value(), 0) + for index in range(3): + EXPECT_EQ(command_interfaces[index].get_value(), 0) + state = system_hw.activate() + EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) + EXPECT_EQ(ACTIVE, state.label()) + + ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) + ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + + # Initiate error on write (this is the second time therefore unrecoverable) + for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): + ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + + state = system_hw.get_state() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) + + # can not change state anymore + state = system_hw.configure() + EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) + EXPECT_EQ(FINALIZED, state.label()) diff --git a/tests/hardware_interface/test_float_ref.py b/tests/hardware_interface/test_float_ref.py index 7a46372..1d0cf33 100644 --- a/tests/hardware_interface/test_float_ref.py +++ b/tests/hardware_interface/test_float_ref.py @@ -114,3 +114,7 @@ def test_self_arith(): for i in range(-10, 11): for j in range(-10, 11): assert_self_ariths(i * 1.5, j / 2) + + +def test_isnan(): + assert isnan(FloatRef(nan)) From 658756f077dce6f3846d82a4b1170dc4c503467a Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Thu, 19 Oct 2023 11:08:32 +0200 Subject: [PATCH 32/42] chore: updated pre-commit && used fixed version for clang-format --- .pre-commit-config.yaml | 20 +++++++++----------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 404cdb5..05e05c7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v4.5.0 hooks: - id: check-added-large-files - id: check-case-conflict @@ -33,21 +33,19 @@ repos: - id: trailing-whitespace - repo: https://github.com/psf/black - rev: 23.3.0 + rev: 23.10.0 hooks: - id: black - - repo: local + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v17.0.3 hooks: - - id: clang-format - name: clang-format - description: Format files with ClangFormat. - entry: clang-format - language: system - files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ - args: ['-i'] + - id: clang-format + args: [-i] + types_or: [c++] + - repo: https://github.com/codespell-project/codespell - rev: v2.2.4 + rev: v2.2.6 hooks: - id: codespell args: ['--write-changes', '--ignore-words=.codespell_words', '--skip="*.eps"'] From 54fb261b10a62595908bc1b138c319ec7b979245 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Thu, 19 Oct 2023 15:22:33 +0200 Subject: [PATCH 33/42] refracto: cleaner arch --- .gitmodules | 4 +- CMakeLists.txt | 19 +- builder/CMakeLists.txt | 11 + cppparser => builder/cppparser | 0 .../src}/main.cpp | 8 +- builder/src/parse.hpp | 10 + .../parse.hpp => builder/src/parse.hxx | 65 +-- .../src}/structs.hpp | 22 +- builder/src/utils.hpp | 53 +++ builder/src/utils.hxx | 140 +++++++ builder/src/utils/hash.hpp | 19 + builder/src/utils/hash.hxx | 65 +++ builder/src/utils/ptr_iter.hpp | 28 ++ builder/src/utils/ptr_iter.hxx | 136 ++++++ builder/src/utils/sep.hpp | 17 + builder/src/utils/sep.hxx | 26 ++ builder/src/write.hpp | 10 + .../write.hpp => builder/src/write.hxx | 85 ++-- src/ros2_control_py_builder/utils.hpp | 390 ------------------ 19 files changed, 613 insertions(+), 495 deletions(-) create mode 100644 builder/CMakeLists.txt rename cppparser => builder/cppparser (100%) rename {src/ros2_control_py_builder => builder/src}/main.cpp (98%) create mode 100644 builder/src/parse.hpp rename src/ros2_control_py_builder/parse.hpp => builder/src/parse.hxx (92%) rename {src/ros2_control_py_builder => builder/src}/structs.hpp (93%) create mode 100644 builder/src/utils.hpp create mode 100644 builder/src/utils.hxx create mode 100644 builder/src/utils/hash.hpp create mode 100644 builder/src/utils/hash.hxx create mode 100644 builder/src/utils/ptr_iter.hpp create mode 100644 builder/src/utils/ptr_iter.hxx create mode 100644 builder/src/utils/sep.hpp create mode 100644 builder/src/utils/sep.hxx create mode 100644 builder/src/write.hpp rename src/ros2_control_py_builder/write.hpp => builder/src/write.hxx (98%) delete mode 100644 src/ros2_control_py_builder/utils.hpp diff --git a/.gitmodules b/.gitmodules index bd009d9..8699815 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ -[submodule "cppparser"] - path = cppparser +[submodule "builder/cppparser"] + path = builder/cppparser url = https://github.com/VokunGahrotLaas/fork-cppparser diff --git a/CMakeLists.txt b/CMakeLists.txt index 51a3e0a..32c6ca6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,31 +22,24 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) -add_subdirectory(cppparser) - -set(_ros2_py_src_dir ${CMAKE_CURRENT_BINARY_DIR}/src) - set(_ros2_py_modules hardware_interface controller_interface ros2_control_test_assets ) +set(_ros2_py_src_dir ${CMAKE_CURRENT_BINARY_DIR}/src) + set(_ros2_py_src ${_ros2_py_modules}) list(TRANSFORM _ros2_py_src PREPEND ${_ros2_py_src_dir}/) list(TRANSFORM _ros2_py_src APPEND _py.cpp) -add_executable(${PROJECT_NAME}_builder - src/${PROJECT_NAME}_builder/main.cpp -) -target_link_libraries(${PROJECT_NAME}_builder - PRIVATE - cppparser -) +add_subdirectory(builder) + add_custom_command( OUTPUT ${_ros2_py_src} - COMMAND ${PROJECT_NAME}_builder "${CMAKE_CURRENT_BINARY_DIR}" "/opt/ros/$ENV{ROS_DISTRO}/include" "$ENV{ROS_DISTRO}" ${_ros2_py_modules} - DEPENDS ${PROJECT_NAME}_builder + COMMAND builder "${_ros2_py_src_dir}" "/opt/ros/$ENV{ROS_DISTRO}/include" "$ENV{ROS_DISTRO}" ${_ros2_py_modules} + DEPENDS builder ) configure_file("${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}/__init__.py" diff --git a/builder/CMakeLists.txt b/builder/CMakeLists.txt new file mode 100644 index 0000000..0a2559a --- /dev/null +++ b/builder/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.5) + +add_subdirectory(cppparser) + +add_executable(builder + src/main.cpp +) +target_link_libraries(builder + PRIVATE + cppparser +) diff --git a/cppparser b/builder/cppparser similarity index 100% rename from cppparser rename to builder/cppparser diff --git a/src/ros2_control_py_builder/main.cpp b/builder/src/main.cpp similarity index 98% rename from src/ros2_control_py_builder/main.cpp rename to builder/src/main.cpp index 82e4556..656ac97 100644 --- a/src/ros2_control_py_builder/main.cpp +++ b/builder/src/main.cpp @@ -10,8 +10,11 @@ void init_py_utils(Module& mod); /// @brief find each classes' mother void init_cls_mothers(Module& mod); +/// @brief inherit virtual members from parent void init_cls_vmembs(Module& mod); +/// @brief find same name functions void init_overloads(Module& mod); +/// @brief resolve header dependencies void init_header_order(Module& mod); int main(int argc, char** argv) { @@ -19,12 +22,11 @@ int main(int argc, char** argv) { "Invalid number of command line arguments, expected at least 4 got " << argc - 1); - const fs::path dst_dir = argv[1]; + const fs::path src_dir = argv[1]; const fs::path inc_dir = argv[2]; const std::string ros_distro = argv[3]; - const fs::path src_dir = dst_dir / "src"; - fs::create_directories(src_dir); + fs::create_directories(src_dir); ASSERT_DIR(src_dir); ASSERT_DIR(inc_dir); ASSERT(ros_distro == "humble" || ros_distro == "rolling", diff --git a/builder/src/parse.hpp b/builder/src/parse.hpp new file mode 100644 index 0000000..1a68920 --- /dev/null +++ b/builder/src/parse.hpp @@ -0,0 +1,10 @@ +#pragma once + +// ros2_control_py_builder +#include "structs.hpp" +#include "utils.hpp" + +inline void parse_header(Module& mod, fs::path path, const std::string& name); + +// hxx +#include "parse.hxx" diff --git a/src/ros2_control_py_builder/parse.hpp b/builder/src/parse.hxx similarity index 92% rename from src/ros2_control_py_builder/parse.hpp rename to builder/src/parse.hxx index 649a2a8..7780821 100644 --- a/src/ros2_control_py_builder/parse.hpp +++ b/builder/src/parse.hxx @@ -1,7 +1,8 @@ #pragma once -// boost -#include +// hpp +#include "parse.hpp" + // CppParser #include #include @@ -10,11 +11,44 @@ #include #include #include -// ros2_control_py_builder -#include "structs.hpp" +// ros2_control_builder #include "utils.hpp" -namespace fs = boost::filesystem; +inline void find_stl(Header& header, const std::string& type_name, + const std::string& type, const std::string& cpp_type); +inline void find_stls(Header& header, const std::string& type_name); +inline void parse_class_attr(Cls& cls, CppConstVarEPtr attr); +inline void parse_class_ctor(Cls& cls, CppConstructorEPtr ctor); +inline void parse_class_memb(Cls& cls, CppConstFunctionEPtr memb); +inline void parse_class_using(Cls& cls); +inline void parse_class(Header& header, CppConstCompoundEPtr cls, + const std::string& ns); +inline void parse_enum(Header& header, CppEnumEPtr enu); +inline void parse_var(Header& header, CppVarEPtr var); +inline void parse_func(Header& header, CppFunctionEPtr func); +inline void parse_namespace(Header& header, CppConstCompoundEPtr ns, + std::string name); + +inline void parse_header(Module& mod, fs::path path, const std::string& name) { + CppParser parser; + std::string const upper_name = to_upper(mod.name); + parser.addIgnorableMacro(upper_name + "_PUBLIC"); + parser.addIgnorableMacro(upper_name + "_LOCAL"); + parser.addIgnorableMacro(upper_name + "_EXPORT"); + parser.addIgnorableMacro(upper_name + "_IMPORT"); + + mod.headers.emplace_back( + std::make_shared
(name.substr(0, name.rfind('.')))); + + const CppCompoundPtr ast = parse_file(parser, path.string()); + ASSERT(ast, "Could not parse " << path); + // std::cerr << "Parsed " << path << std::endl; + for (const CppObjPtr& obj_ns : ast->members()) { + CppConstCompoundEPtr ns = obj_ns; + if (!ns || !isNamespace(ns) || ns->name() != mod.name) continue; + parse_namespace(*mod.headers.back(), ns, ns->name()); + } +} inline void find_stl(Header& header, const std::string& type_name, const std::string& type, const std::string& cpp_type) { @@ -283,24 +317,3 @@ inline void parse_namespace(Header& header, CppConstCompoundEPtr ns, parse_class(header, compound, name); } } - -inline void parse_header(Module& mod, fs::path path, const std::string& name) { - CppParser parser; - std::string const upper_name = to_upper(mod.name); - parser.addIgnorableMacro(upper_name + "_PUBLIC"); - parser.addIgnorableMacro(upper_name + "_LOCAL"); - parser.addIgnorableMacro(upper_name + "_EXPORT"); - parser.addIgnorableMacro(upper_name + "_IMPORT"); - - mod.headers.emplace_back( - std::make_shared
(name.substr(0, name.rfind('.')))); - - const CppCompoundPtr ast = parse_file(parser, path.string()); - ASSERT(ast, "Could not parse " << path); - // std::cerr << "Parsed " << path << std::endl; - for (const CppObjPtr& obj_ns : ast->members()) { - CppConstCompoundEPtr ns = obj_ns; - if (!ns || !isNamespace(ns) || ns->name() != mod.name) continue; - parse_namespace(*mod.headers.back(), ns, ns->name()); - } -} diff --git a/src/ros2_control_py_builder/structs.hpp b/builder/src/structs.hpp similarity index 93% rename from src/ros2_control_py_builder/structs.hpp rename to builder/src/structs.hpp index f8f53f7..ca16f27 100644 --- a/src/ros2_control_py_builder/structs.hpp +++ b/builder/src/structs.hpp @@ -5,13 +5,9 @@ #include #include #include -// boost -#include // ros2_control_py_builder #include "utils.hpp" -namespace fs = boost::filesystem; - struct Var { Var(const std::string& name) : name{name} {}; @@ -28,10 +24,10 @@ struct Func { args_type{std::move(args_type)}, args_names{std::move(args_names)} {} - friend bool operator==(const Func& lhs, const Func& rhs) { + friend inline bool operator==(const Func& lhs, const Func& rhs) { return lhs.name == rhs.name && lhs.args_type == rhs.args_type; } - friend bool operator!=(const Func& lhs, const Func& rhs) { + friend inline bool operator!=(const Func& lhs, const Func& rhs) { return !(lhs == rhs); } @@ -53,10 +49,10 @@ struct Attr : public Var { struct Ctor { Ctor(std::vector&& args) : args{std::move(args)} {} - friend bool operator==(const Ctor& lhs, const Ctor& rhs) { + friend inline bool operator==(const Ctor& lhs, const Ctor& rhs) { return lhs.args == rhs.args; } - friend bool operator!=(const Ctor& lhs, const Ctor& rhs) { + friend inline bool operator!=(const Ctor& lhs, const Ctor& rhs) { return !(lhs == rhs); } @@ -78,7 +74,7 @@ struct Memb : public Func { is_final{is_final}, is_public{is_public} {} - Memb clone(const std::string& new_cls) const { + inline Memb clone(const std::string& new_cls) const { auto new_args = args; auto new_args_type = args_type; auto new_args_names = args_names; @@ -95,11 +91,11 @@ struct Memb : public Func { is_public}; } - friend bool operator==(const Memb& lhs, const Memb& rhs) { + friend inline bool operator==(const Memb& lhs, const Memb& rhs) { return lhs.is_const == rhs.is_const && (dynamic_cast(lhs) == dynamic_cast(rhs)); } - friend bool operator!=(const Memb& lhs, const Memb& rhs) { + friend inline bool operator!=(const Memb& lhs, const Memb& rhs) { return !(lhs == rhs); } @@ -268,7 +264,7 @@ struct Module { const fs::path inc_dir; const fs::path src_dir; const fs::path src; - std::string name; + const std::string name; std::vector> headers{}; - std::shared_ptr
py_utils; + const std::shared_ptr
py_utils; }; diff --git a/builder/src/utils.hpp b/builder/src/utils.hpp new file mode 100644 index 0000000..036a97a --- /dev/null +++ b/builder/src/utils.hpp @@ -0,0 +1,53 @@ +#pragma once + +// STL +#include +#include +// boost +#include +// CppParser +#include + +namespace fs = boost::filesystem; + +/// @brief if not `Assert` print the other args and exit failure +#define ASSERT(Assert, ...) \ + do { \ + if (Assert) break; \ + std::cerr << __VA_ARGS__ << std::endl; \ + std::exit(EXIT_FAILURE); \ + } while (false) +/// @brief if not fs::is_directori((Dir)) fails +#define ASSERT_DIR(Dir) \ + ASSERT(fs::is_directory((Dir)), (Dir) << " is not a valid directory") + +/// @brief removes `[[.*]]` sequences from the string because the parser does +/// not handle them well +inline void remove_attributes(std::string& contents); +/// @brief same as readFile from CppParser but calls remove_attributes +inline std::string read_file(const std::string& filename); +/// @brief same as parseFile from CppParser but calls read_file instead of +/// readFile +inline CppCompoundPtr parse_file(CppParser& parser, + const std::string& filename); +/// @brief wrapper to get a string repr of a CppObj +inline std::string str_of_cpp(const CppObj* cppObj); +/// @brief std::string to UPPER +inline std::string to_upper(std::string_view str); +/// @brief std::string_view to PascalCase +inline std::string to_pascal_case(std::string_view str); +/// @brief std::string_view(s) to just_name, to PascalCase +template +inline std::string make_pascal_name(const Args&... args); +/// @brief remove everything before the last occcurence of `::` (not in place) +inline std::string just_name(const std::string& name); +/// @brief remove everything before the last occcurence of `::` (in place) +inline std::string just_name(std::string&& name); + +// utils/ +#include "utils/hash.hpp" +#include "utils/ptr_iter.hpp" +#include "utils/sep.hpp" + +// hxx +#include "utils.hxx" diff --git a/builder/src/utils.hxx b/builder/src/utils.hxx new file mode 100644 index 0000000..45f9840 --- /dev/null +++ b/builder/src/utils.hxx @@ -0,0 +1,140 @@ +#pragma once + +// hpp +#include "utils.hpp" + +// STL +#include +#include +#include +#include +// CppParser +#include + +inline void remove_attributes(std::string& contents) { + // remove attributes aka [[...]] + auto it = contents.cbegin(); + while (it != contents.cend()) { + it = std::search_n(it, contents.cend(), 2, '['); + auto end = std::search_n(it, contents.cend(), 2, ']'); + if (end != contents.cend()) it = contents.erase(it, end + 2); + } + // remove digit separators aka d'ddd'ddd but not char digits aka 'd' + it = contents.cbegin(); + while (it != contents.cend()) { + it = std::adjacent_find(it, contents.cend(), [](char a, char b) { + return std::isdigit(a) && b == '\''; + }); + if (it == contents.cend()) continue; + it += 2; + if (it == contents.cend() || !std::isdigit(*it)) continue; + it = contents.erase(it - 1); + } + // remove raw strings aka R"()" + auto mit = contents.begin(); + while (mit != contents.end()) { + mit = std::adjacent_find(mit, contents.end(), [](char a, char b) { + return a == 'R' && b == '"'; + }); + if (mit == contents.end()) continue; + mit += 2; + if (mit == contents.end()) continue; + auto name_end = std::find(mit, contents.end(), '('); + std::string name = std::string{mit, name_end}; + auto valid_dchar = [](char c) { + return (!std::isalnum(c) && !std::ispunct(c)) || (c == ')' || c == '\\'); + }; + if (name.size() > 16 || + std::find_if(name.cbegin(), name.cend(), valid_dchar) != name.cend()) + continue; + std::size_t end = + contents.find(")" + name + "\"", name_end - contents.cbegin()); + if (end == std::string::npos) continue; + *(mit - 2) = '"'; // replpace R with " + mit = contents.erase( + mit - 1, contents.cbegin() + end + name.size() + 1); // R|"name()name|" + } +} + +inline std::string read_file(const std::string& filename) { + std::string contents; + std::ifstream in(filename, std::ios::in | std::ios::binary); + if (in) { + in.seekg(0, std::ios::end); + size_t size = static_cast(in.tellg()); + contents.resize(size + 3); // For adding last 2 nulls and a new line. + in.seekg(0, std::ios::beg); + in.read(contents.data(), size); + in.close(); + auto len = stripChar(contents.data(), size, '\r'); + assert(len <= size); + remove_attributes(contents); + contents.resize(len + 3); + contents[len] = '\n'; + contents[len + 1] = '\0'; + contents[len + 2] = '\0'; + } + return contents; +} + +inline CppCompoundPtr parse_file(CppParser& parser, + const std::string& filename) { + std::string stm = read_file(filename); + CppCompoundPtr cppCompound = parser.parseStream(stm.data(), stm.size()); + if (!cppCompound) return cppCompound; + cppCompound->name(filename); + return cppCompound; +} + +inline std::string str_of_cpp(const CppObj* cppObj) { + std::ostringstream oss; + CppWriter{}.emit(cppObj, oss); + std::string str = std::move(oss).str(); + if (!str.empty() && str.back() == '*') { + std::string type = str.substr(0, str.size() - 1); + if (type == "double") return "Ref<" + type + ">"; + } + return str; +} + +inline std::string to_upper(std::string str) { + std::transform(str.cbegin(), str.cend(), str.begin(), + [](unsigned char c) { return std::toupper(c); }); + return str; +} + +inline std::string to_pascal_case(std::string_view str) { + std::string r; + r.resize(str.size()); + auto rit = r.begin(); + for (auto it = str.cbegin(); it != str.cend(); ++it) { + auto end = std::find(it, str.cend(), '_'); + if (it == end) continue; + *rit++ = std::toupper(static_cast(*it++)); + if (it != end) { + std::copy(it, end, rit); + rit += end - it; + } + it = end; + if (it == str.cend()) break; + } + if (rit != r.cend()) r.erase(rit, r.cend()); + return r; +} + +template +inline std::string make_pascal_name(const Args&... args) { + std::string r; + (r.append(to_pascal_case(just_name(args))), ...); + return r; +} + +inline std::string just_name(const std::string& name) { + return just_name(std::string{name}); +} + +inline std::string just_name(std::string&& name) { + auto it = std::search_n(name.crbegin(), name.crend(), 2, ':'); + if (it != name.crend()) name.erase(name.cbegin(), it.base()); + return name; +} diff --git a/builder/src/utils/hash.hpp b/builder/src/utils/hash.hpp new file mode 100644 index 0000000..f7cafe4 --- /dev/null +++ b/builder/src/utils/hash.hpp @@ -0,0 +1,19 @@ +#pragma once + +// STL +#include + +/// @brief combines seed with `std::hash{}(data)` +template +void combine_hash(std::size_t& seed, const T& data); + +/// @brief std::hash for all std::pair +template +struct std::hash>; + +/// @brief std::hash for all std::tuple +template +struct std::hash>; + +// hxx +#include "hash.hxx" diff --git a/builder/src/utils/hash.hxx b/builder/src/utils/hash.hxx new file mode 100644 index 0000000..ee678ea --- /dev/null +++ b/builder/src/utils/hash.hxx @@ -0,0 +1,65 @@ +#pragma once + +// hpp +#include "hash.hpp" + +// STL +#include + +inline std::uint64_t hash_mix(std::uint64_t x); +inline std::uint32_t hash_mix(std::uint32_t x); + +template +void combine_hash(std::size_t& seed, const T& data) { + seed = hash_mix(seed + 0x9e3779b9 + std::hash{}(data)); +} + +template +struct std::hash> { + constexpr std::uint64_t operator()(const std::pair& pair) const { + std::size_t h = std::hash{}(pair.first); + combine_hash(h, pair.second); + return h; + } +}; + +template +struct std::hash> { + constexpr std::uint64_t operator()(const std::tuple& tuple) const { + return impl(tuple, std::index_sequence_for{}); + } + + private: + template + constexpr std::uint64_t impl(const std::tuple& tuple, + std::index_sequence) const { + std::size_t h = 0; + (combine_hash(h, std::get(tuple)), ...); + return h; + } +}; + +inline std::uint64_t hash_mix(std::uint64_t x) { + const std::uint64_t m = (std::uint64_t(0xe9846af) << 32) + 0x9b1a615d; + + x ^= x >> 32; + x *= m; + x ^= x >> 32; + x *= m; + x ^= x >> 28; + + return x; +} + +inline std::uint32_t hash_mix(std::uint32_t x) { + const std::uint32_t m1 = 0x21f0aaad; + const std::uint32_t m2 = 0x735a2d97; + + x ^= x >> 16; + x *= m1; + x ^= x >> 15; + x *= m2; + x ^= x >> 15; + + return x; +} diff --git a/builder/src/utils/ptr_iter.hpp b/builder/src/utils/ptr_iter.hpp new file mode 100644 index 0000000..f4efe90 --- /dev/null +++ b/builder/src/utils/ptr_iter.hpp @@ -0,0 +1,28 @@ +#pragma once + +/// @brief wrapper over an iterator `T` to pointer-like that acts as an iterator +/// to the pointed objects +template +class PtrIt; +/// @brief range of `const T&` container with PtrIt as iterators +template +class CPtrIterable; +/// @brief range of `T&` container with PtrIt as iterators +template +class PtrIterable; +/// @brief range of `T` container with PtrIt as iterators +template +class OwningPtrIterable; + +/// @brief creates the right kind of PtrIterable +template +OwningPtrIterable ptr_iter(T&& iterable); +/// @brief creates the right kind of PtrIterable +template +PtrIterable ptr_iter(T& iterable); +/// @brief creates the right kind of PtrIterable +template +CPtrIterable ptr_iter(const T& iterable); + +// hxx +#include "ptr_iter.hxx" diff --git a/builder/src/utils/ptr_iter.hxx b/builder/src/utils/ptr_iter.hxx new file mode 100644 index 0000000..b3f93c1 --- /dev/null +++ b/builder/src/utils/ptr_iter.hxx @@ -0,0 +1,136 @@ +#pragma once + +// hpp +#include "ptr_iter.hpp" + +// STL +#include + +template +class PtrIt { + public: + explicit PtrIt(T it) : it_{it} {} + + decltype(std::declval()->operator*()) operator*() const { + return it_->operator*(); + } + auto operator->() const { return std::addressof(operator*()); } + + PtrIt operator++() { return PtrIt{++it_}; } + PtrIt operator++(int) { return PtrIt{it_++}; } + + PtrIt operator--() { return PtrIt{--it_}; } + PtrIt operator--(int) { return PtrIt{it_--}; } + + PtrIt operator+(std::ptrdiff_t i) const { return PtrIt{it_} += i; } + PtrIt& operator+=(std::ptrdiff_t i) { + it_ += i; + return *this; + } + + PtrIt operator-(std::ptrdiff_t i) const { return PtrIt{it_} -= i; } + PtrIt& operator-=(std::ptrdiff_t i) { + it_ -= i; + return *this; + } + + bool operator==(const PtrIt& other) const { return it_ == other.it_; } + bool operator!=(const PtrIt& other) const { return !(*this == other); } + + T base() const { return it_; } + void from_base(T it) { it_ = it; } + + private: + T it_; +}; + +template +class CPtrIterable { + public: + explicit CPtrIterable(const T& iterable) noexcept : iterable_{iterable} {} + + auto begin() const { return PtrIt{std::begin(iterable_)}; } + auto end() const { return PtrIt{std::end(iterable_)}; } + + auto cbegin() const { return PtrIt{std::cbegin(iterable_)}; } + auto cend() const { return PtrIt{std::cend(iterable_)}; } + + auto rbegin() const { return PtrIt{std::rbegin(iterable_)}; } + auto rend() const { return PtrIt{std::rend(iterable_)}; } + + auto crbegin() const { return PtrIt{std::crbegin(iterable_)}; } + auto crend() const { return PtrIt{std::crend(iterable_)}; } + + private: + const T& iterable_; +}; + +template +class PtrIterable { + public: + explicit PtrIterable(T& iterable) noexcept : iterable_{iterable} {} + + auto begin() { return PtrIt{std::begin(iterable_)}; } + auto end() { return PtrIt{std::end(iterable_)}; } + + auto begin() const { return PtrIt{std::begin(iterable_)}; } + auto end() const { return PtrIt{std::end(iterable_)}; } + + auto cbegin() const { return PtrIt{std::cbegin(iterable_)}; } + auto cend() const { return PtrIt{std::cend(iterable_)}; } + + auto rbegin() { return PtrIt{std::rbegin(iterable_)}; } + auto rend() { return PtrIt{std::rend(iterable_)}; } + + auto rbegin() const { return PtrIt{std::rbegin(iterable_)}; } + auto rend() const { return PtrIt{std::rend(iterable_)}; } + + auto crbegin() const { return PtrIt{std::crbegin(iterable_)}; } + auto crend() const { return PtrIt{std::crend(iterable_)}; } + + private: + T& iterable_; +}; + +template +class OwningPtrIterable { + public: + explicit OwningPtrIterable(T&& iterable) noexcept + : iterable_{std::move(iterable)} {} + + auto begin() { return PtrIt{std::begin(iterable_)}; } + auto end() { return PtrIt{std::end(iterable_)}; } + + auto begin() const { return PtrIt{std::begin(iterable_)}; } + auto end() const { return PtrIt{std::end(iterable_)}; } + + auto cbegin() const { return PtrIt{std::cbegin(iterable_)}; } + auto cend() const { return PtrIt{std::cend(iterable_)}; } + + auto rbegin() { return PtrIt{std::rbegin(iterable_)}; } + auto rend() { return PtrIt{std::rend(iterable_)}; } + + auto rbegin() const { return PtrIt{std::rbegin(iterable_)}; } + auto rend() const { return PtrIt{std::rend(iterable_)}; } + + auto crbegin() const { return PtrIt{std::crbegin(iterable_)}; } + auto crend() const { return PtrIt{std::crend(iterable_)}; } + + private: + T iterable_; +}; + +template +OwningPtrIterable ptr_iter(T&& iterable) { + return OwningPtrIterable{std::move(iterable)}; +} + +template +PtrIterable ptr_iter(T& iterable) { + return PtrIterable{iterable}; +} + +template +CPtrIterable ptr_iter(const T& iterable) { + return CPtrIterable{iterable}; +} diff --git a/builder/src/utils/sep.hpp b/builder/src/utils/sep.hpp new file mode 100644 index 0000000..36a3adf --- /dev/null +++ b/builder/src/utils/sep.hpp @@ -0,0 +1,17 @@ +#pragma once + +// STL +#include + +/// @brief wrapper over a `const T&` and a `const U&`, display all elements of a +/// `T` with a 'U' as a separator +/// @expample std::cout << Sep{std::vector{1, 2, 3}, ", "} << std::endl; // +/// output: `1, 2, 3` +template +class Sep; +/// @brief ostream writer for `Sep` +template +std::ostream& operator<<(std::ostream& os, const Sep& sep); + +// hxx +#include "sep.hxx" diff --git a/builder/src/utils/sep.hxx b/builder/src/utils/sep.hxx new file mode 100644 index 0000000..8289e41 --- /dev/null +++ b/builder/src/utils/sep.hxx @@ -0,0 +1,26 @@ +#pragma once + +// hpp +#include "sep.hpp" + +template +class Sep { + public: + Sep(const T& iterable, const U& separator) + : iterable_{iterable}, separator_{separator} {} + + friend std::ostream& operator<< <>(std::ostream&, const Sep&); + + private: + const T& iterable_; + const U& separator_; +}; + +template +std::ostream& operator<<(std::ostream& os, const Sep& sep) { + auto it = std::begin(sep.iterable_); + if (it == std::end(sep.iterable_)) return os; + os << *it; + for (++it; it != std::end(sep.iterable_); ++it) os << sep.separator_ << *it; + return os; +} diff --git a/builder/src/write.hpp b/builder/src/write.hpp new file mode 100644 index 0000000..07cf8f6 --- /dev/null +++ b/builder/src/write.hpp @@ -0,0 +1,10 @@ +#pragma once + +// ros2_control_pŷ_builder +#include "structs.hpp" + +/// @brief write source files for module `mod` +inline void write_module(const Module& mod); + +// hxx +#include "write.hxx" diff --git a/src/ros2_control_py_builder/write.hpp b/builder/src/write.hxx similarity index 98% rename from src/ros2_control_py_builder/write.hpp rename to builder/src/write.hxx index aecd520..6298bff 100644 --- a/src/ros2_control_py_builder/write.hpp +++ b/builder/src/write.hxx @@ -1,14 +1,7 @@ #pragma once -// STL -#include -// boost -#include -// ros2_control_pŷ_builder -#include "structs.hpp" -#include "utils.hpp" - -namespace fs = boost::filesystem; +// hpp +#include "write.hpp" /// @brief output a `Cls` to write an hpp inline std::ostream& operator<<(std::ostream& os, const Cls& cls); @@ -22,10 +15,42 @@ inline std::ostream& operator<<(std::ostream& os, const Var& var); inline std::ostream& operator<<(std::ostream& os, const Func& func); /// @brief write source files for module `mod` and header `header` inline void write_module_header(const Module& mod, const Header& header); -/// @brief write source files for module `mod` -inline void write_module(const Module& mod); -// Impl +void write_module(const Module& mod) { + for (const Header& header : ptr_iter(mod.headers)) + write_module_header(mod, header); + + std::ofstream ofs{mod.src, std::ios::out | std::ios::trunc}; + ASSERT(ofs, "could not open " << mod.src); + ofs << R"(// pybind11 +#include + +// )" << mod.name + << '\n'; + for (const Header& header : ptr_iter(mod.headers)) + ofs << "#include <" << mod.name << "/" << header.name << "_py.hpp>\n"; + ofs << R"( +namespace py = pybind11; + +PYBIND11_MODULE()" + << mod.name << R"(, m) +{ + m.doc() = R"doc( + Python bindings for ros2_control functionalities. + )doc"; + + // Provide custom function signatures + py::options options; + options.disable_function_signatures(); + + // Construct module classes +)"; + for (const Header& header : ptr_iter(mod.headers)) { + ofs << " ros2_control_py::bind_" << mod.name << "::init_" + << header.proper_name << "(m);\n"; + } + ofs << "}\n"; +} inline std::ostream& operator<<(std::ostream& os, const Cls& cls) { os << " py::class_<" << (cls.is_outsider ? cls.complete_name : cls.name); @@ -420,39 +445,3 @@ inline void init_)" } )"; } - -void write_module(const Module& mod) { - for (const Header& header : ptr_iter(mod.headers)) - write_module_header(mod, header); - - std::ofstream ofs{mod.src, std::ios::out | std::ios::trunc}; - ASSERT(ofs, "could not open " << mod.src); - ofs << R"(// pybind11 -#include - -// )" << mod.name - << '\n'; - for (const Header& header : ptr_iter(mod.headers)) - ofs << "#include <" << mod.name << "/" << header.name << "_py.hpp>\n"; - ofs << R"( -namespace py = pybind11; - -PYBIND11_MODULE()" - << mod.name << R"(, m) -{ - m.doc() = R"doc( - Python bindings for ros2_control functionalities. - )doc"; - - // Provide custom function signatures - py::options options; - options.disable_function_signatures(); - - // Construct module classes -)"; - for (const Header& header : ptr_iter(mod.headers)) { - ofs << " ros2_control_py::bind_" << mod.name << "::init_" - << header.proper_name << "(m);\n"; - } - ofs << "}\n"; -} diff --git a/src/ros2_control_py_builder/utils.hpp b/src/ros2_control_py_builder/utils.hpp deleted file mode 100644 index ca19ee2..0000000 --- a/src/ros2_control_py_builder/utils.hpp +++ /dev/null @@ -1,390 +0,0 @@ -#pragma once - -// STL -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -// CppParser -#include -#include - -/// @brief if not `Assert` print the other args and exit failure -#define ASSERT(Assert, ...) \ - do { \ - if (Assert) break; \ - std::cerr << __VA_ARGS__ << std::endl; \ - std::exit(EXIT_FAILURE); \ - } while (false) -/// @brief if not fs::is_directori((Dir)) fails -#define ASSERT_DIR(Dir) \ - ASSERT(fs::is_directory((Dir)), (Dir) << " is not a valid directory") - -/// @brief wrapper over a `const T&` and a `const U&`, display all elements of a -/// `T` with a 'U' as a separator -/// @expample std::cout << Sep{std::vector{1, 2, 3}, ", "} << std::endl; // -/// output: `1, 2, 3` -template -class Sep; - -/// @brief removes `[[.*]]` sequences from the string because the parser does -/// not handle them well -inline void remove_attributes(std::string& contents); -/// @brief same as readFile from CppParser but calls remove_attributes -inline std::string read_file(const std::string& filename); -/// @brief same as parseFile from CppParser but calls read_file instead of -/// readFile -inline CppCompoundPtr parse_file(CppParser& parser, - const std::string& filename); -/// @brief ostream writer for `Sep` -template -std::ostream& operator<<(std::ostream& os, const Sep& sep); -/// @brief wrapper to get a string repr of a CppObj -inline std::string str_of_cpp(CppWriter& writer, const CppObj* cppObj); -/// @brief std::string to upper (not in place) -inline std::string to_upper(std::string str); -inline std::string just_name(const std::string& name); -inline std::string just_name(std::string&& name); - -// Impl - -inline void remove_attributes(std::string& contents) { - // remove attributes aka [[...]] - auto it = contents.cbegin(); - while (it != contents.cend()) { - it = std::search_n(it, contents.cend(), 2, '['); - auto end = std::search_n(it, contents.cend(), 2, ']'); - if (end != contents.cend()) it = contents.erase(it, end + 2); - } - // remove digit separators aka d'ddd'ddd but not char digits aka 'd' - it = contents.cbegin(); - while (it != contents.cend()) { - it = std::adjacent_find(it, contents.cend(), [](char a, char b) { - return std::isdigit(a) && b == '\''; - }); - if (it == contents.cend()) continue; - it += 2; - if (it == contents.cend() || !std::isdigit(*it)) continue; - it = contents.erase(it - 1); - } - // remove raw strings aka R"()" - auto mit = contents.begin(); - while (mit != contents.end()) { - mit = std::adjacent_find(mit, contents.end(), [](char a, char b) { - return a == 'R' && b == '"'; - }); - if (mit == contents.end()) continue; - mit += 2; - if (mit == contents.end()) continue; - auto name_end = std::find(mit, contents.end(), '('); - std::string name = std::string{mit, name_end}; - auto valid_dchar = [](char c) { - return (!std::isalnum(c) && !std::ispunct(c)) || (c == ')' || c == '\\'); - }; - if (name.size() > 16 || - std::find_if(name.cbegin(), name.cend(), valid_dchar) != name.cend()) - continue; - std::size_t end = - contents.find(")" + name + "\"", name_end - contents.cbegin()); - if (end == std::string::npos) continue; - *(mit - 2) = '"'; // replpace R with " - mit = contents.erase( - mit - 1, contents.cbegin() + end + name.size() + 1); // R|"name()name|" - } -} - -inline std::string read_file(const std::string& filename) { - std::string contents; - std::ifstream in(filename, std::ios::in | std::ios::binary); - if (in) { - in.seekg(0, std::ios::end); - size_t size = static_cast(in.tellg()); - contents.resize(size + 3); // For adding last 2 nulls and a new line. - in.seekg(0, std::ios::beg); - in.read(contents.data(), size); - in.close(); - auto len = stripChar(contents.data(), size, '\r'); - assert(len <= size); - remove_attributes(contents); - contents.resize(len + 3); - contents[len] = '\n'; - contents[len + 1] = '\0'; - contents[len + 2] = '\0'; - } - return contents; -} - -inline CppCompoundPtr parse_file(CppParser& parser, - const std::string& filename) { - std::string stm = read_file(filename); - CppCompoundPtr cppCompound = parser.parseStream(stm.data(), stm.size()); - if (!cppCompound) return cppCompound; - cppCompound->name(filename); - return cppCompound; -} - -template -class Sep { - public: - Sep(const T& iterable, const U& separator) - : iterable_{iterable}, separator_{separator} {} - - friend std::ostream& operator<< <>(std::ostream&, const Sep&); - - private: - const T& iterable_; - const U& separator_; -}; - -template -std::ostream& operator<<(std::ostream& os, const Sep& sep) { - auto it = std::begin(sep.iterable_); - if (it == std::end(sep.iterable_)) return os; - os << *it; - for (++it; it != std::end(sep.iterable_); ++it) os << sep.separator_ << *it; - return os; -} - -inline std::string str_of_cpp(const CppObj* cppObj) { - std::ostringstream oss; - CppWriter{}.emit(cppObj, oss); - std::string str = std::move(oss).str(); - if (!str.empty() && str.back() == '*') { - std::string type = str.substr(0, str.size() - 1); - if (type == "double") return "Ref<" + type + ">"; - } - return str; -} - -inline std::string to_upper(std::string str) { - std::transform(str.cbegin(), str.cend(), str.begin(), - [](char c) { return std::toupper(c); }); - return str; -} - -inline std::string to_pascal_case(std::string_view str) { - std::string r; - r.resize(str.size()); - auto rit = r.begin(); - for (auto it = str.cbegin(); it != str.cend(); ++it) { - auto end = std::find(it, str.cend(), '_'); - if (it == end) continue; - *rit++ = std::toupper(*it++); - if (it != end) { - std::copy(it, end, rit); - rit += end - it; - } - it = end; - if (it == str.cend()) break; - } - if (rit != r.cend()) r.erase(rit, r.cend()); - return r; -} - -template -inline std::string make_pascal_name(const Args&... args) { - std::string r; - (r.append(to_pascal_case(just_name(args))), ...); - return r; -} - -inline std::string just_name(const std::string& name) { - return just_name(std::string{name}); -} - -inline std::string just_name(std::string&& name) { - auto it = std::search_n(name.crbegin(), name.crend(), 2, ':'); - if (it != name.crend()) name.erase(name.cbegin(), it.base()); - return std::string{std::move(name)}; -} - -template -class PtrIt { - public: - explicit PtrIt(T it) : it_{it} {} - - decltype(std::declval()->operator*()) operator*() const { - return it_->operator*(); - } - auto operator->() const { return std::addressof(this->operator*()); } - - PtrIt operator++() { return PtrIt{++it_}; } - PtrIt operator++(int) { return PtrIt{it_++}; } - - PtrIt operator--() { return PtrIt{--it_}; } - PtrIt operator--(int) { return PtrIt{it_--}; } - - PtrIt operator+(std::ptrdiff_t i) const { return PtrIt{it_} += i; } - PtrIt& operator+=(std::ptrdiff_t i) { - it_ += i; - return *this; - } - - PtrIt operator-(std::ptrdiff_t i) const { return PtrIt{it_} -= i; } - PtrIt& operator-=(std::ptrdiff_t i) { - it_ -= i; - return *this; - } - - bool operator==(const PtrIt& other) const { return it_ == other.it_; } - bool operator!=(const PtrIt& other) const { return !(*this == other); } - - T base() const { return it_; } - void from_base(T it) { it_ = it; } - - private: - T it_; -}; - -template -class CPtrIterable { - public: - explicit CPtrIterable(const T& iterable) noexcept : iterable_{iterable} {} - - auto begin() const { return PtrIt{std::begin(iterable_)}; } - auto end() const { return PtrIt{std::end(iterable_)}; } - - auto cbegin() const { return PtrIt{std::cbegin(iterable_)}; } - auto cend() const { return PtrIt{std::cend(iterable_)}; } - - auto rbegin() const { return PtrIt{std::rbegin(iterable_)}; } - auto rend() const { return PtrIt{std::rend(iterable_)}; } - - auto crbegin() const { return PtrIt{std::crbegin(iterable_)}; } - auto crend() const { return PtrIt{std::crend(iterable_)}; } - - private: - const T& iterable_; -}; - -template -class PtrIterable { - public: - explicit PtrIterable(T& iterable) noexcept : iterable_{iterable} {} - - auto begin() { return PtrIt{std::begin(iterable_)}; } - auto end() { return PtrIt{std::end(iterable_)}; } - - auto begin() const { return PtrIt{std::begin(iterable_)}; } - auto end() const { return PtrIt{std::end(iterable_)}; } - - auto cbegin() const { return PtrIt{std::cbegin(iterable_)}; } - auto cend() const { return PtrIt{std::cend(iterable_)}; } - - auto rbegin() { return PtrIt{std::rbegin(iterable_)}; } - auto rend() { return PtrIt{std::rend(iterable_)}; } - - auto rbegin() const { return PtrIt{std::rbegin(iterable_)}; } - auto rend() const { return PtrIt{std::rend(iterable_)}; } - - auto crbegin() const { return PtrIt{std::crbegin(iterable_)}; } - auto crend() const { return PtrIt{std::crend(iterable_)}; } - - private: - T& iterable_; -}; - -template -class OwningPtrIterable { - public: - explicit OwningPtrIterable(T&& iterable) noexcept - : iterable_{std::move(iterable)} {} - - auto begin() { return PtrIt{std::begin(iterable_)}; } - auto end() { return PtrIt{std::end(iterable_)}; } - - auto begin() const { return PtrIt{std::begin(iterable_)}; } - auto end() const { return PtrIt{std::end(iterable_)}; } - - auto cbegin() const { return PtrIt{std::cbegin(iterable_)}; } - auto cend() const { return PtrIt{std::cend(iterable_)}; } - - auto rbegin() { return PtrIt{std::rbegin(iterable_)}; } - auto rend() { return PtrIt{std::rend(iterable_)}; } - - auto rbegin() const { return PtrIt{std::rbegin(iterable_)}; } - auto rend() const { return PtrIt{std::rend(iterable_)}; } - - auto crbegin() const { return PtrIt{std::crbegin(iterable_)}; } - auto crend() const { return PtrIt{std::crend(iterable_)}; } - - private: - T iterable_; -}; - -template -auto ptr_iter(T&& iterable) { - return OwningPtrIterable{std::move(iterable)}; -} - -template -auto ptr_iter(T& iterable) { - return PtrIterable{iterable}; -} - -template -auto ptr_iter(const T& iterable) { - return CPtrIterable{iterable}; -} - -inline std::uint64_t fn(std::uint64_t x) { - const std::uint64_t m = (std::uint64_t(0xe9846af) << 32) + 0x9b1a615d; - - x ^= x >> 32; - x *= m; - x ^= x >> 32; - x *= m; - x ^= x >> 28; - - return x; -} - -inline std::uint32_t hash_mix(std::uint32_t x) { - const std::uint32_t m1 = 0x21f0aaad; - const std::uint32_t m2 = 0x735a2d97; - - x ^= x >> 16; - x *= m1; - x ^= x >> 15; - x *= m2; - x ^= x >> 15; - - return x; -} - -template -void combine_hash(std::size_t& seed, const T& data) { - seed = hash_mix(seed + 0x9e3779b9 + std::hash{}(data)); -} - -template -struct std::hash> { - constexpr std::uint64_t operator()(const std::pair& pair) const { - std::size_t h = std::hash{}(pair.first); - combine_hash(h, pair.second); - return h; - } -}; - -template -struct std::hash> { - constexpr std::uint64_t operator()(const std::tuple& tuple) const { - return impl(tuple, std::index_sequence_for{}); - } - - private: - template - constexpr std::uint64_t impl(const std::tuple& tuple, - std::index_sequence) const { - std::size_t h = 0; - (combine_hash(h, std::get(tuple)), ...); - return h; - } -}; From cc3069031e37f32f00b9559bca8a70ebe0aca71c Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 20 Oct 2023 10:40:19 +0200 Subject: [PATCH 34/42] chore: added comments to cmake --- CMakeLists.txt | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 32c6ca6..46e08b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.20) project(ros2_control_py VERSION 0.0.2) +## default ros std versions/warnings # Default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) @@ -15,13 +16,19 @@ if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +# ROS deps find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(rclpy REQUIRED) +# Python deps find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) +## compiles builder +add_subdirectory(builder) + +## Vars set(_ros2_py_modules hardware_interface controller_interface @@ -34,21 +41,13 @@ set(_ros2_py_src ${_ros2_py_modules}) list(TRANSFORM _ros2_py_src PREPEND ${_ros2_py_src_dir}/) list(TRANSFORM _ros2_py_src APPEND _py.cpp) -add_subdirectory(builder) - add_custom_command( OUTPUT ${_ros2_py_src} COMMAND builder "${_ros2_py_src_dir}" "/opt/ros/$ENV{ROS_DISTRO}/include" "$ENV{ROS_DISTRO}" ${_ros2_py_modules} DEPENDS builder ) -configure_file("${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}/__init__.py" - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}/__init__.py" - COPYONLY -) -ament_python_install_package(${PROJECT_NAME} - PACKAGE_DIR "${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}" -) +# python modules foreach(_module ${_ros2_py_modules}) find_package(${_module} REQUIRED) pybind11_add_module(${_module}_py @@ -68,6 +67,15 @@ foreach(_module ${_ros2_py_modules}) DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" ) endforeach() +# __init__ in build/ for tests +configure_file("${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}/__init__.py" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}/__init__.py" + COPYONLY +) +# __init__ in install/ +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR "${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}" +) if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) From 523c8692d57d6a4f51615dae94e3feec19464db2 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 20 Oct 2023 11:16:05 +0200 Subject: [PATCH 35/42] feat: update gitmodules && put ament_python_install_package before modules so it can export PYTHON_INSTALL_DIR --- .gitmodules | 2 +- CMakeLists.txt | 19 ++++++++++--------- builder/cppparser | 2 +- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/.gitmodules b/.gitmodules index 8699815..eed3ceb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "builder/cppparser"] path = builder/cppparser - url = https://github.com/VokunGahrotLaas/fork-cppparser + url = https://github.com/Gepetto/cppparser diff --git a/CMakeLists.txt b/CMakeLists.txt index 46e08b9..1decd59 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,6 +47,15 @@ add_custom_command( DEPENDS builder ) +# __init__ in build/ for tests +configure_file("${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}/__init__.py" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}/__init__.py" + COPYONLY +) +# __init__ in install/ +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR "${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}" +) # python modules foreach(_module ${_ros2_py_modules}) find_package(${_module} REQUIRED) @@ -63,19 +72,11 @@ foreach(_module ${_ros2_py_modules}) rclpy ${_module} ) + # ${PYTHON_INSTALL_DIR} is exported by ament_python_install_package() above install(TARGETS ${_module}_py DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" ) endforeach() -# __init__ in build/ for tests -configure_file("${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}/__init__.py" - "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}/__init__.py" - COPYONLY -) -# __init__ in install/ -ament_python_install_package(${PROJECT_NAME} - PACKAGE_DIR "${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}" -) if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) diff --git a/builder/cppparser b/builder/cppparser index 1ffb755..157aaca 160000 --- a/builder/cppparser +++ b/builder/cppparser @@ -1 +1 @@ -Subproject commit 1ffb7550b29f38ded92679bb92c2982875595ae4 +Subproject commit 157aaca0db5e95909a908c8fb97dc1927e552ce2 From 887b1efc60b17225ecfd1553f18352f98995749e Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 20 Oct 2023 14:08:19 +0200 Subject: [PATCH 36/42] fix(builder): typo --- builder/src/utils/sep.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/builder/src/utils/sep.hpp b/builder/src/utils/sep.hpp index 36a3adf..e0e34e3 100644 --- a/builder/src/utils/sep.hpp +++ b/builder/src/utils/sep.hpp @@ -5,8 +5,8 @@ /// @brief wrapper over a `const T&` and a `const U&`, display all elements of a /// `T` with a 'U' as a separator -/// @expample std::cout << Sep{std::vector{1, 2, 3}, ", "} << std::endl; // -/// output: `1, 2, 3` +/// @example std::cout << Sep{std::vector{1, 2, 3}, ", "} << std::endl; +/// // output: `1, 2, 3` template class Sep; /// @brief ostream writer for `Sep` From 6cba06fe40d1d0d442743b122d3418098357f5e8 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 20 Oct 2023 14:11:56 +0200 Subject: [PATCH 37/42] fix(builder): added include --- builder/src/main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/builder/src/main.cpp b/builder/src/main.cpp index 656ac97..e3aa172 100644 --- a/builder/src/main.cpp +++ b/builder/src/main.cpp @@ -1,5 +1,6 @@ // STL #include +#include #include // ros2_control_py_builder #include "parse.hpp" From b2cf394f7e9b225a25becfb4cabf73dee87b36d7 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 20 Oct 2023 15:19:19 +0200 Subject: [PATCH 38/42] chore: replaced gtest macro with python assert --- .../test_component_interface.py | 538 ++++++++---------- 1 file changed, 252 insertions(+), 286 deletions(-) diff --git a/tests/hardware_interface/test_component_interface.py b/tests/hardware_interface/test_component_interface.py index 8786a0f..cedcf84 100644 --- a/tests/hardware_interface/test_component_interface.py +++ b/tests/hardware_interface/test_component_interface.py @@ -262,110 +262,92 @@ def perform_command_mode_switch( return return_type.OK -def ASSERT_TRUE(a): - assert a - - -def EXPECT_TRUE(a): - ASSERT_TRUE(a) - - -def ASSERT_EQ(a, b): - assert a == b - - -def EXPECT_EQ(a, b): - ASSERT_EQ(a, b) - - def test_dummy_actuator(): actuator_hw = Actuator(DummyActuator()) mock_hw_info = HardwareInfo() state = actuator_hw.initialize(mock_hw_info) - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() state_interfaces = actuator_hw.export_state_interfaces() - ASSERT_EQ(2, len(state_interfaces)) - EXPECT_EQ("joint1/position", state_interfaces[0].get_name()) - EXPECT_EQ(HW_IF_POSITION, state_interfaces[0].get_interface_name()) - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()) - EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()) - EXPECT_EQ(HW_IF_VELOCITY, state_interfaces[1].get_interface_name()) - EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()) + assert 2 == len(state_interfaces) + assert "joint1/position" == state_interfaces[0].get_name() + assert HW_IF_POSITION == state_interfaces[0].get_interface_name() + assert "joint1" == state_interfaces[0].get_prefix_name() + assert "joint1/velocity" == state_interfaces[1].get_name() + assert HW_IF_VELOCITY == state_interfaces[1].get_interface_name() + assert "joint1" == state_interfaces[1].get_prefix_name() command_interfaces = actuator_hw.export_command_interfaces() - ASSERT_EQ(1, len(command_interfaces)) - EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()) - EXPECT_EQ(HW_IF_VELOCITY, command_interfaces[0].get_interface_name()) - EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()) + assert 1 == len(command_interfaces) + assert "joint1/velocity" == command_interfaces[0].get_name() + assert HW_IF_VELOCITY == command_interfaces[0].get_interface_name() + assert "joint1" == command_interfaces[0].get_prefix_name() velocity_value = 1.0 command_interfaces[0].set_value(velocity_value) - ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + assert return_type.ERROR == actuator_hw.write(TIME, PERIOD) # Noting should change because it is UNCONFIGURED for step in range(10): - ASSERT_EQ(return_type.ERROR, actuator_hw.read(TIME, PERIOD)) + assert return_type.ERROR == actuator_hw.read(TIME, PERIOD) - ASSERT_TRUE(isnan(state_interfaces[0].get_value())) # position value - ASSERT_TRUE(isnan(state_interfaces[1].get_value())) # velocity + assert isnan(state_interfaces[0].get_value()) # position value + assert isnan(state_interfaces[1].get_value()) # velocity - ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + assert return_type.ERROR == actuator_hw.write(TIME, PERIOD) state = actuator_hw.configure() - EXPECT_EQ(State.PRIMARY_STATE_INACTIVE, state.id()) - EXPECT_EQ(INACTIVE, state.label()) + assert State.PRIMARY_STATE_INACTIVE == state.id() + assert INACTIVE == state.label() # Read and Write are working because it is INACTIVE for step in range(10): - ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + assert return_type.OK == actuator_hw.read(TIME, PERIOD) - EXPECT_EQ(step * velocity_value, state_interfaces[0].get_value()) + assert step * velocity_value == state_interfaces[0].get_value() # position value - EXPECT_EQ(velocity_value if step else 0, state_interfaces[1].get_value()) + assert velocity_value if step else 0 == state_interfaces[1].get_value() # velocity - ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + assert return_type.OK == actuator_hw.write(TIME, PERIOD) state = actuator_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() # Read and Write are working because it is ACTIVE for step in range(10): - ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) + assert return_type.OK == actuator_hw.read(TIME, PERIOD) - EXPECT_EQ((10 + step) * velocity_value, state_interfaces[0].get_value()) + assert (10 + step) * velocity_value == state_interfaces[0].get_value() # position value - EXPECT_EQ(velocity_value, state_interfaces[1].get_value()) + assert velocity_value == state_interfaces[1].get_value() # velocity - ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + assert return_type.OK == actuator_hw.write(TIME, PERIOD) state = actuator_hw.shutdown() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() # Noting should change because it is FINALIZED for step in range(10): - ASSERT_EQ(return_type.ERROR, actuator_hw.read(TIME, PERIOD)) + assert return_type.ERROR == actuator_hw.read(TIME, PERIOD) - EXPECT_EQ(20 * velocity_value, state_interfaces[0].get_value()) + assert 20 * velocity_value == state_interfaces[0].get_value() # position value - EXPECT_EQ(0, state_interfaces[1].get_value()) + assert 0 == state_interfaces[1].get_value() # velocity - ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + assert return_type.ERROR == actuator_hw.write(TIME, PERIOD) - EXPECT_EQ( - return_type.OK, - actuator_hw.prepare_command_mode_switch(VectorString([""]), VectorString([""])), + assert return_type.OK == actuator_hw.prepare_command_mode_switch( + VectorString([""]), VectorString([""]) ) - EXPECT_EQ( - return_type.OK, - actuator_hw.perform_command_mode_switch(VectorString([""]), VectorString([""])), + assert return_type.OK == actuator_hw.perform_command_mode_switch( + VectorString([""]), VectorString([""]) ) @@ -374,29 +356,29 @@ def test_dummy_sensor(): mock_hw_info = HardwareInfo() state = sensor_hw.initialize(mock_hw_info) - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() state_interfaces = sensor_hw.export_state_interfaces() - ASSERT_EQ(1, len(state_interfaces)) - EXPECT_EQ("joint1/voltage", state_interfaces[0].get_name()) - EXPECT_EQ("voltage", state_interfaces[0].get_interface_name()) - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()) - EXPECT_TRUE(isnan(state_interfaces[0].get_value())) + assert 1 == len(state_interfaces) + assert "joint1/voltage" == state_interfaces[0].get_name() + assert "voltage" == state_interfaces[0].get_interface_name() + assert "joint1" == state_interfaces[0].get_prefix_name() + assert isnan(state_interfaces[0].get_value()) # Not updated because is is UNCONFIGURED sensor_hw.read(TIME, PERIOD) - EXPECT_TRUE(isnan(state_interfaces[0].get_value())) + assert isnan(state_interfaces[0].get_value()) # Updated because is is INACTIVE state = sensor_hw.configure() - EXPECT_EQ(State.PRIMARY_STATE_INACTIVE, state.id()) - EXPECT_EQ(INACTIVE, state.label()) - EXPECT_EQ(0, state_interfaces[0].get_value()) + assert State.PRIMARY_STATE_INACTIVE == state.id() + assert INACTIVE == state.label() + assert 0 == state_interfaces[0].get_value() # It can read now sensor_hw.read(TIME, PERIOD) - EXPECT_EQ(0x666, state_interfaces[0].get_value()) + assert 0x666 == state_interfaces[0].get_value() def test_dummy_system(): @@ -404,143 +386,135 @@ def test_dummy_system(): mock_hw_info = HardwareInfo() state = system_hw.initialize(mock_hw_info) - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() state_interfaces = system_hw.export_state_interfaces() - ASSERT_EQ(6, len(state_interfaces)) - EXPECT_EQ("joint1/position", state_interfaces[0].get_name()) - EXPECT_EQ(HW_IF_POSITION, state_interfaces[0].get_interface_name()) - EXPECT_EQ("joint1", state_interfaces[0].get_prefix_name()) - EXPECT_EQ("joint1/velocity", state_interfaces[1].get_name()) - EXPECT_EQ(HW_IF_VELOCITY, state_interfaces[1].get_interface_name()) - EXPECT_EQ("joint1", state_interfaces[1].get_prefix_name()) - EXPECT_EQ("joint2/position", state_interfaces[2].get_name()) - EXPECT_EQ(HW_IF_POSITION, state_interfaces[2].get_interface_name()) - EXPECT_EQ("joint2", state_interfaces[2].get_prefix_name()) - EXPECT_EQ("joint2/velocity", state_interfaces[3].get_name()) - EXPECT_EQ(HW_IF_VELOCITY, state_interfaces[3].get_interface_name()) - EXPECT_EQ("joint2", state_interfaces[3].get_prefix_name()) - EXPECT_EQ("joint3/position", state_interfaces[4].get_name()) - EXPECT_EQ(HW_IF_POSITION, state_interfaces[4].get_interface_name()) - EXPECT_EQ("joint3", state_interfaces[4].get_prefix_name()) - EXPECT_EQ("joint3/velocity", state_interfaces[5].get_name()) - EXPECT_EQ(HW_IF_VELOCITY, state_interfaces[5].get_interface_name()) - EXPECT_EQ("joint3", state_interfaces[5].get_prefix_name()) + assert 6 == len(state_interfaces) + assert "joint1/position" == state_interfaces[0].get_name() + assert HW_IF_POSITION == state_interfaces[0].get_interface_name() + assert "joint1" == state_interfaces[0].get_prefix_name() + assert "joint1/velocity" == state_interfaces[1].get_name() + assert HW_IF_VELOCITY == state_interfaces[1].get_interface_name() + assert "joint1" == state_interfaces[1].get_prefix_name() + assert "joint2/position" == state_interfaces[2].get_name() + assert HW_IF_POSITION == state_interfaces[2].get_interface_name() + assert "joint2" == state_interfaces[2].get_prefix_name() + assert "joint2/velocity" == state_interfaces[3].get_name() + assert HW_IF_VELOCITY == state_interfaces[3].get_interface_name() + assert "joint2" == state_interfaces[3].get_prefix_name() + assert "joint3/position" == state_interfaces[4].get_name() + assert HW_IF_POSITION == state_interfaces[4].get_interface_name() + assert "joint3" == state_interfaces[4].get_prefix_name() + assert "joint3/velocity" == state_interfaces[5].get_name() + assert HW_IF_VELOCITY == state_interfaces[5].get_interface_name() + assert "joint3" == state_interfaces[5].get_prefix_name() command_interfaces = system_hw.export_command_interfaces() - ASSERT_EQ(3, len(command_interfaces)) - EXPECT_EQ("joint1/velocity", command_interfaces[0].get_name()) - EXPECT_EQ(HW_IF_VELOCITY, command_interfaces[0].get_interface_name()) - EXPECT_EQ("joint1", command_interfaces[0].get_prefix_name()) - EXPECT_EQ("joint2/velocity", command_interfaces[1].get_name()) - EXPECT_EQ(HW_IF_VELOCITY, command_interfaces[1].get_interface_name()) - EXPECT_EQ("joint2", command_interfaces[1].get_prefix_name()) - EXPECT_EQ("joint3/velocity", command_interfaces[2].get_name()) - EXPECT_EQ(HW_IF_VELOCITY, command_interfaces[2].get_interface_name()) - EXPECT_EQ("joint3", command_interfaces[2].get_prefix_name()) + assert 3 == len(command_interfaces) + assert "joint1/velocity" == command_interfaces[0].get_name() + assert HW_IF_VELOCITY == command_interfaces[0].get_interface_name() + assert "joint1" == command_interfaces[0].get_prefix_name() + assert "joint2/velocity" == command_interfaces[1].get_name() + assert HW_IF_VELOCITY == command_interfaces[1].get_interface_name() + assert "joint2" == command_interfaces[1].get_prefix_name() + assert "joint3/velocity" == command_interfaces[2].get_name() + assert HW_IF_VELOCITY == command_interfaces[2].get_interface_name() + assert "joint3" == command_interfaces[2].get_prefix_name() velocity_value = 1.0 command_interfaces[0].set_value(velocity_value) # velocity command_interfaces[1].set_value(velocity_value) # velocity command_interfaces[2].set_value(velocity_value) # velocity - ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + assert return_type.ERROR == system_hw.write(TIME, PERIOD) # Noting should change because it is UNCONFIGURED for step in range(10): - ASSERT_EQ(return_type.ERROR, system_hw.read(TIME, PERIOD)) + assert return_type.ERROR == system_hw.read(TIME, PERIOD) - ASSERT_TRUE(isnan(state_interfaces[0].get_value())) # position value - ASSERT_TRUE(isnan(state_interfaces[1].get_value())) # velocity - ASSERT_TRUE(isnan(state_interfaces[2].get_value())) # position value - ASSERT_TRUE(isnan(state_interfaces[3].get_value())) # velocity - ASSERT_TRUE(isnan(state_interfaces[4].get_value())) # position value - ASSERT_TRUE(isnan(state_interfaces[5].get_value())) # velocity + assert isnan(state_interfaces[0].get_value()) # position value + assert isnan(state_interfaces[1].get_value()) # velocity + assert isnan(state_interfaces[2].get_value()) # position value + assert isnan(state_interfaces[3].get_value()) # velocity + assert isnan(state_interfaces[4].get_value()) # position value + assert isnan(state_interfaces[5].get_value()) # velocity - ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + assert return_type.ERROR == system_hw.write(TIME, PERIOD) state = system_hw.configure() - EXPECT_EQ(State.PRIMARY_STATE_INACTIVE, state.id()) - EXPECT_EQ(INACTIVE, state.label()) + assert State.PRIMARY_STATE_INACTIVE == state.id() + assert INACTIVE == state.label() # Read and Write are working because it is INACTIVE for step in range(10): - ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) + assert return_type.OK == system_hw.read(TIME, PERIOD) - EXPECT_EQ( - step * velocity_value, state_interfaces[0].get_value() + assert ( + step * velocity_value == state_interfaces[0].get_value() ) # position value - EXPECT_EQ( - velocity_value if step else 0, state_interfaces[1].get_value() + assert ( + velocity_value if step else 0 == state_interfaces[1].get_value() ) # velocity - EXPECT_EQ( - step * velocity_value, state_interfaces[2].get_value() + assert ( + step * velocity_value == state_interfaces[2].get_value() ) # position value - EXPECT_EQ( - velocity_value if step else 0, state_interfaces[3].get_value() + assert ( + velocity_value if step else 0 == state_interfaces[3].get_value() ) # velocity - EXPECT_EQ( - step * velocity_value, state_interfaces[4].get_value() + assert ( + step * velocity_value == state_interfaces[4].get_value() ) # position value - EXPECT_EQ( - velocity_value if step else 0, state_interfaces[5].get_value() + assert ( + velocity_value if step else 0 == state_interfaces[5].get_value() ) # velocity - ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + assert return_type.OK == system_hw.write(TIME, PERIOD) state = system_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() # Read and Write are working because it is ACTIVE for step in range(10): - ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) - - EXPECT_EQ( - (10 + step) * velocity_value, state_interfaces[0].get_value() - ) # position value - EXPECT_EQ(velocity_value, state_interfaces[1].get_value()) # velocity - EXPECT_EQ( - (10 + step) * velocity_value, state_interfaces[2].get_value() - ) # position value - EXPECT_EQ(velocity_value, state_interfaces[3].get_value()) # velocity - EXPECT_EQ( - (10 + step) * velocity_value, state_interfaces[4].get_value() - ) # position value - EXPECT_EQ(velocity_value, state_interfaces[5].get_value()) # velocity - - ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + assert return_type.OK == system_hw.read(TIME, PERIOD) + + assert (10 + step) * velocity_value == state_interfaces[ + 0 + ].get_value() # position value + assert velocity_value == state_interfaces[1].get_value() # velocity + assert (10 + step) * velocity_value == state_interfaces[ + 2 + ].get_value() # position value + assert velocity_value == state_interfaces[3].get_value() # velocity + assert (10 + step) * velocity_value == state_interfaces[ + 4 + ].get_value() # position value + assert velocity_value == state_interfaces[5].get_value() # velocity + + assert return_type.OK == system_hw.write(TIME, PERIOD) state = system_hw.shutdown() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() # Noting should change because it is FINALIZED for step in range(10): - ASSERT_EQ(return_type.ERROR, system_hw.read(TIME, PERIOD)) + assert return_type.ERROR == system_hw.read(TIME, PERIOD) - EXPECT_EQ( - 20 * velocity_value, state_interfaces[0].get_value() - ) # position value - EXPECT_EQ(0, state_interfaces[1].get_value()) # velocity - EXPECT_EQ( - 20 * velocity_value, state_interfaces[2].get_value() - ) # position value - EXPECT_EQ(0, state_interfaces[3].get_value()) # velocity - EXPECT_EQ( - 20 * velocity_value, state_interfaces[4].get_value() - ) # position value - EXPECT_EQ(0, state_interfaces[5].get_value()) # velocity + assert 20 * velocity_value == state_interfaces[0].get_value() # position value + assert 0 == state_interfaces[1].get_value() # velocity + assert 20 * velocity_value == state_interfaces[2].get_value() # position value + assert 0 == state_interfaces[3].get_value() # velocity + assert 20 * velocity_value == state_interfaces[4].get_value() # position value + assert 0 == state_interfaces[5].get_value() # velocity - ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + assert return_type.ERROR == system_hw.write(TIME, PERIOD) - EXPECT_EQ( - return_type.OK, - system_hw.prepare_command_mode_switch(VectorString([""]), VectorString([""])), + assert return_type.OK == system_hw.prepare_command_mode_switch( + VectorString([""]), VectorString([""]) ) - EXPECT_EQ( - return_type.OK, - system_hw.perform_command_mode_switch(VectorString([""]), VectorString([""])), + assert return_type.OK == system_hw.perform_command_mode_switch( + VectorString([""]), VectorString([""]) ) @@ -549,27 +523,19 @@ def test_dummy_command_mode_system(): mock_hw_info = HardwareInfo() state = system_hw.initialize(mock_hw_info) - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() one_key = VectorString(["joint1/position"]) two_keys = VectorString(["joint1/position", "joint1/velocity"]) # Only calls with (one_key, two_keys) should return OK - EXPECT_EQ( - return_type.ERROR, system_hw.prepare_command_mode_switch(one_key, one_key) - ) - EXPECT_EQ( - return_type.ERROR, system_hw.perform_command_mode_switch(one_key, one_key) - ) - EXPECT_EQ(return_type.OK, system_hw.prepare_command_mode_switch(one_key, two_keys)) - EXPECT_EQ(return_type.OK, system_hw.perform_command_mode_switch(one_key, two_keys)) - EXPECT_EQ( - return_type.ERROR, system_hw.prepare_command_mode_switch(two_keys, one_key) - ) - EXPECT_EQ( - return_type.ERROR, system_hw.perform_command_mode_switch(two_keys, one_key) - ) + assert return_type.ERROR, system_hw.prepare_command_mode_switch(one_key == one_key) + assert return_type.ERROR, system_hw.perform_command_mode_switch(one_key == one_key) + assert return_type.OK == system_hw.prepare_command_mode_switch(one_key, two_keys) + assert return_type.OK == system_hw.perform_command_mode_switch(one_key, two_keys) + assert return_type.ERROR, system_hw.prepare_command_mode_switch(two_keys == one_key) + assert return_type.ERROR, system_hw.perform_command_mode_switch(two_keys == one_key) def test_dummy_actuator_read_error_behavior(): @@ -577,53 +543,53 @@ def test_dummy_actuator_read_error_behavior(): mock_hw_info = HardwareInfo() state = actuator_hw.initialize(mock_hw_info) - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() state_interfaces = actuator_hw.export_state_interfaces() command_interfaces = actuator_hw.export_command_interfaces() state = actuator_hw.configure() state = actuator_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() - ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + assert return_type.OK == actuator_hw.read(TIME, PERIOD) + assert return_type.OK == actuator_hw.write(TIME, PERIOD) # Initiate error on write (this is first time therefore recoverable) for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, actuator_hw.read(TIME, PERIOD)) + assert return_type.OK == actuator_hw.read(TIME, PERIOD) + assert return_type.ERROR == actuator_hw.read(TIME, PERIOD) state = actuator_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() # activate again and expect reset values state = actuator_hw.configure() - EXPECT_EQ(state_interfaces[0].get_value(), 0) - EXPECT_EQ(command_interfaces[0].get_value(), 0) + assert state_interfaces[0].get_value() == 0 + assert command_interfaces[0].get_value() == 0 state = actuator_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() - ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + assert return_type.OK == actuator_hw.read(TIME, PERIOD) + assert return_type.OK == actuator_hw.write(TIME, PERIOD) # Initiate error on write (this is the second time therefore unrecoverable) for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, actuator_hw.read(TIME, PERIOD)) + assert return_type.OK == actuator_hw.read(TIME, PERIOD) + assert return_type.ERROR == actuator_hw.read(TIME, PERIOD) state = actuator_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() # can not change state anymore state = actuator_hw.configure() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() def test_dummy_actuator_write_error_behavior(): @@ -631,53 +597,53 @@ def test_dummy_actuator_write_error_behavior(): mock_hw_info = HardwareInfo() state = actuator_hw.initialize(mock_hw_info) - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() state_interfaces = actuator_hw.export_state_interfaces() command_interfaces = actuator_hw.export_command_interfaces() state = actuator_hw.configure() state = actuator_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() - ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + assert return_type.OK == actuator_hw.read(TIME, PERIOD) + assert return_type.OK == actuator_hw.write(TIME, PERIOD) # Initiate error on write (this is first time therefore recoverable) for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + assert return_type.OK == actuator_hw.write(TIME, PERIOD) + assert return_type.ERROR == actuator_hw.write(TIME, PERIOD) state = actuator_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() # activate again and expect reset values state = actuator_hw.configure() - EXPECT_EQ(state_interfaces[0].get_value(), 0) - EXPECT_EQ(command_interfaces[0].get_value(), 0) + assert state_interfaces[0].get_value() == 0 + assert command_interfaces[0].get_value() == 0 state = actuator_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() - ASSERT_EQ(return_type.OK, actuator_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) + assert return_type.OK == actuator_hw.read(TIME, PERIOD) + assert return_type.OK == actuator_hw.write(TIME, PERIOD) # Initiate error on write (this is the second time therefore unrecoverable) for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, actuator_hw.write(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, actuator_hw.write(TIME, PERIOD)) + assert return_type.OK == actuator_hw.write(TIME, PERIOD) + assert return_type.ERROR == actuator_hw.write(TIME, PERIOD) state = actuator_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() # can not change state anymore state = actuator_hw.configure() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() def test_dummy_sensor_read_error_behavior(): @@ -690,41 +656,41 @@ def test_dummy_sensor_read_error_behavior(): # Updated because is is INACTIVE state = sensor_hw.configure() state = sensor_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() - ASSERT_EQ(return_type.OK, sensor_hw.read(TIME, PERIOD)) + assert return_type.OK == sensor_hw.read(TIME, PERIOD) # Initiate recoverable error - call read 99 times OK and on 100-time will return error for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, sensor_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, sensor_hw.read(TIME, PERIOD)) + assert return_type.OK == sensor_hw.read(TIME, PERIOD) + assert return_type.ERROR == sensor_hw.read(TIME, PERIOD) state = sensor_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() # activate again and expect reset values state = sensor_hw.configure() - EXPECT_EQ(state_interfaces[0].get_value(), 0) + assert state_interfaces[0].get_value() == 0 state = sensor_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() # Initiate unrecoverable error - call read 99 times OK and on 100-time will return error for i in range(1, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, sensor_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, sensor_hw.read(TIME, PERIOD)) + assert return_type.OK == sensor_hw.read(TIME, PERIOD) + assert return_type.ERROR == sensor_hw.read(TIME, PERIOD) state = sensor_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() # can not change state anymore state = sensor_hw.configure() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() def test_dummy_system_read_error_behavior(): @@ -732,54 +698,54 @@ def test_dummy_system_read_error_behavior(): mock_hw_info = HardwareInfo() state = system_hw.initialize(mock_hw_info) - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() state_interfaces = system_hw.export_state_interfaces() command_interfaces = system_hw.export_command_interfaces() state = system_hw.configure() state = system_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() - ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + assert return_type.OK == system_hw.read(TIME, PERIOD) + assert return_type.OK == system_hw.write(TIME, PERIOD) # Initiate error on write (this is first time therefore recoverable) for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, system_hw.read(TIME, PERIOD)) + assert return_type.OK == system_hw.read(TIME, PERIOD) + assert return_type.ERROR == system_hw.read(TIME, PERIOD) state = system_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() # activate again and expect reset values state = system_hw.configure() for index in range(6): - EXPECT_EQ(state_interfaces[index].get_value(), 0) + assert state_interfaces[index].get_value() == 0 for index in range(3): - EXPECT_EQ(command_interfaces[index].get_value(), 0) + assert command_interfaces[index].get_value() == 0 state = system_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() - ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + assert return_type.OK == system_hw.read(TIME, PERIOD) + assert return_type.OK == system_hw.write(TIME, PERIOD) # Initiate error on write (this is the second time therefore unrecoverable) for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, system_hw.read(TIME, PERIOD)) + assert return_type.OK == system_hw.read(TIME, PERIOD) + assert return_type.ERROR == system_hw.read(TIME, PERIOD) state = system_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() # can not change state anymore state = system_hw.configure() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() def test_dummy_system_write_error_behavior(): @@ -787,51 +753,51 @@ def test_dummy_system_write_error_behavior(): mock_hw_info = HardwareInfo() state = system_hw.initialize(mock_hw_info) - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() state_interfaces = system_hw.export_state_interfaces() command_interfaces = system_hw.export_command_interfaces() state = system_hw.configure() state = system_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() - ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + assert return_type.OK == system_hw.read(TIME, PERIOD) + assert return_type.OK == system_hw.write(TIME, PERIOD) # Initiate error on write (this is first time therefore recoverable) for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + assert return_type.OK == system_hw.write(TIME, PERIOD) + assert return_type.ERROR == system_hw.write(TIME, PERIOD) state = system_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_UNCONFIGURED, state.id()) - EXPECT_EQ(UNCONFIGURED, state.label()) + assert State.PRIMARY_STATE_UNCONFIGURED == state.id() + assert UNCONFIGURED == state.label() # activate again and expect reset values state = system_hw.configure() for index in range(6): - EXPECT_EQ(state_interfaces[index].get_value(), 0) + assert state_interfaces[index].get_value() == 0 for index in range(3): - EXPECT_EQ(command_interfaces[index].get_value(), 0) + assert command_interfaces[index].get_value() == 0 state = system_hw.activate() - EXPECT_EQ(State.PRIMARY_STATE_ACTIVE, state.id()) - EXPECT_EQ(ACTIVE, state.label()) + assert State.PRIMARY_STATE_ACTIVE == state.id() + assert ACTIVE == state.label() - ASSERT_EQ(return_type.OK, system_hw.read(TIME, PERIOD)) - ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) + assert return_type.OK == system_hw.read(TIME, PERIOD) + assert return_type.OK == system_hw.write(TIME, PERIOD) # Initiate error on write (this is the second time therefore unrecoverable) for i in range(2, TRIGGER_READ_WRITE_ERROR_CALLS): - ASSERT_EQ(return_type.OK, system_hw.write(TIME, PERIOD)) - ASSERT_EQ(return_type.ERROR, system_hw.write(TIME, PERIOD)) + assert return_type.OK == system_hw.write(TIME, PERIOD) + assert return_type.ERROR == system_hw.write(TIME, PERIOD) state = system_hw.get_state() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() # can not change state anymore state = system_hw.configure() - EXPECT_EQ(State.PRIMARY_STATE_FINALIZED, state.id()) - EXPECT_EQ(FINALIZED, state.label()) + assert State.PRIMARY_STATE_FINALIZED == state.id() + assert FINALIZED == state.label() From ea1d937bd50caae2323c79e36ec7fb23dd521f50 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Thu, 26 Oct 2023 16:51:32 +0200 Subject: [PATCH 39/42] fix: run the builder once per compilation --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1decd59..2db2d09 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,6 +46,9 @@ add_custom_command( COMMAND builder "${_ros2_py_src_dir}" "/opt/ros/$ENV{ROS_DISTRO}/include" "$ENV{ROS_DISTRO}" ${_ros2_py_modules} DEPENDS builder ) +add_custom_target(run_builder + DEPENDS ${_ros2_py_src} +) # __init__ in build/ for tests configure_file("${CMAKE_CURRENT_LIST_DIR}/python/${PROJECT_NAME}/__init__.py" @@ -62,6 +65,7 @@ foreach(_module ${_ros2_py_modules}) pybind11_add_module(${_module}_py "${_ros2_py_src_dir}/${_module}_py.cpp" ) + add_dependencies(${_module}_py run_builder) set_target_properties(${_module}_py PROPERTIES OUTPUT_NAME ${PROJECT_NAME}/${_module} ) From 5d4006a11a6f36979472a1bf7e9f699ff0e1016a Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Thu, 26 Oct 2023 16:52:52 +0200 Subject: [PATCH 40/42] fix(builder): member overload detection/display --- builder/src/parse.hxx | 4 ++-- builder/src/structs.hpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/builder/src/parse.hxx b/builder/src/parse.hxx index 7780821..3c2273d 100644 --- a/builder/src/parse.hxx +++ b/builder/src/parse.hxx @@ -177,8 +177,8 @@ inline void parse_class_memb(Cls& cls, CppConstFunctionEPtr memb) { cls.membs.emplace_back(std::make_shared( memb->name_, cls.name, ret_type, std::move(args_cpy), std::move(args_type_cpy), std::move(args_names_cpy), - isConst(memb.get()), isVirtual(memb.get()), isPureVirtual(memb.get()), - isFinal(memb.get()), isPublic(memb.get()))); + isConst(memb.get()), false, false, isFinal(memb.get()), + isPublic(memb.get()))); } cls.membs.emplace_back(std::make_shared( memb->name_, cls.name, ret_type, std::move(args), std::move(args_type), diff --git a/builder/src/structs.hpp b/builder/src/structs.hpp index ca16f27..faaa1d7 100644 --- a/builder/src/structs.hpp +++ b/builder/src/structs.hpp @@ -143,6 +143,7 @@ struct Cls { } std::shared_ptr find_mutable_overload(const Memb& memb) const { + if (!memb.is_const) return nullptr; auto it = std::find_if(membs.cbegin(), membs.cend(), [memb](std::shared_ptr other) { return !other->is_const && From b758f1f6e08e3f507ed66b1f9641efed36b5be95 Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 27 Oct 2023 16:51:39 +0200 Subject: [PATCH 41/42] feat(debug): added sanitize flags to cmake --- CMakeLists.txt | 6 ++++++ README.md | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2db2d09..3093450 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,12 @@ if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_C_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +## sanitize address gcc/clang +if(SANITIZE) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fno-omit-frame-pointer -fsanitize=address,undefined,leak") + set(CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS} -fno-omit-frame-pointer -fsanitize=address,undefined,leak") +endif() + # ROS deps find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) diff --git a/README.md b/README.md index 8dfe5bf..83e6180 100644 --- a/README.md +++ b/README.md @@ -25,7 +25,7 @@ colcon build ```sh cd ~/ros2_control_py_ws -colcon build +colcon build --cmake-args ' -DCMAKE_BUILD_TYPE=Debug -DSANITIZE' colcon test ``` From 9c18b6a24f8fdc9be07761a528cec930128d1a5c Mon Sep 17 00:00:00 2001 From: Maxence MICHOT Date: Fri, 27 Oct 2023 16:53:38 +0200 Subject: [PATCH 42/42] feat: added test for ResourceManager && fixed builder to accommodate --- builder/src/parse.hxx | 2 +- builder/src/structs.hpp | 35 +- builder/src/utils.hpp | 2 + builder/src/utils.hxx | 5 + builder/src/utils/sep.hpp | 12 +- builder/src/utils/sep.hxx | 46 +- builder/src/write.hxx | 12 +- .../test_resource_manager.py | 1774 +++++++++++++++++ 8 files changed, 1852 insertions(+), 36 deletions(-) create mode 100644 tests/hardware_interface/test_resource_manager.py diff --git a/builder/src/parse.hxx b/builder/src/parse.hxx index 3c2273d..afc9cf2 100644 --- a/builder/src/parse.hxx +++ b/builder/src/parse.hxx @@ -211,7 +211,7 @@ inline void parse_class(Header& header, CppConstCompoundEPtr cls, } CppConstFunctionEPtr memb = obj_memb; if (memb && memb->name_.find("operator") == std::string::npos && - memb->name_ != "get_full_name" && memb->name_ != "import_component") { + memb->name_ != "get_full_name") { parse_class_memb(*cls_rep, memb); continue; } diff --git a/builder/src/structs.hpp b/builder/src/structs.hpp index faaa1d7..ac90e10 100644 --- a/builder/src/structs.hpp +++ b/builder/src/structs.hpp @@ -24,10 +24,10 @@ struct Func { args_type{std::move(args_type)}, args_names{std::move(args_names)} {} - friend inline bool operator==(const Func& lhs, const Func& rhs) { + friend inline bool operator==(const Func& lhs, const Func& rhs) noexcept { return lhs.name == rhs.name && lhs.args_type == rhs.args_type; } - friend inline bool operator!=(const Func& lhs, const Func& rhs) { + friend inline bool operator!=(const Func& lhs, const Func& rhs) noexcept { return !(lhs == rhs); } @@ -47,12 +47,12 @@ struct Attr : public Var { }; struct Ctor { - Ctor(std::vector&& args) : args{std::move(args)} {} + Ctor(std::vector&& args) noexcept : args{std::move(args)} {} - friend inline bool operator==(const Ctor& lhs, const Ctor& rhs) { + friend inline bool operator==(const Ctor& lhs, const Ctor& rhs) noexcept { return lhs.args == rhs.args; } - friend inline bool operator!=(const Ctor& lhs, const Ctor& rhs) { + friend inline bool operator!=(const Ctor& lhs, const Ctor& rhs) noexcept { return !(lhs == rhs); } @@ -91,11 +91,11 @@ struct Memb : public Func { is_public}; } - friend inline bool operator==(const Memb& lhs, const Memb& rhs) { + friend inline bool operator==(const Memb& lhs, const Memb& rhs) noexcept { return lhs.is_const == rhs.is_const && - (dynamic_cast(lhs) == dynamic_cast(rhs)); + (static_cast(lhs) == static_cast(rhs)); } - friend inline bool operator!=(const Memb& lhs, const Memb& rhs) { + friend inline bool operator!=(const Memb& lhs, const Memb& rhs) noexcept { return !(lhs == rhs); } @@ -123,7 +123,7 @@ struct Cls { mother{std::move(mother)}, init{mother_name.empty()} {} - auto find_overloads(const std::string& name) { + auto find_overloads(const std::string& name) noexcept { std::vector> found; for (auto it = membs.begin(); it != membs.end();) { it = std::find_if(it, membs.end(), @@ -135,14 +135,15 @@ struct Cls { return found; } - std::shared_ptr find_override(const Memb& memb) { + std::shared_ptr find_override(const Memb& memb) noexcept { auto it = std::find_if( membs.begin(), membs.end(), [memb](std::shared_ptr other) { return *other == memb; }); return it != membs.cend() ? *it : nullptr; } - std::shared_ptr find_mutable_overload(const Memb& memb) const { + std::shared_ptr find_mutable_overload( + const Memb& memb) const noexcept { if (!memb.is_const) return nullptr; auto it = std::find_if(membs.cbegin(), membs.cend(), [memb](std::shared_ptr other) { @@ -153,7 +154,7 @@ struct Cls { return it != membs.cend() ? *it : nullptr; } - auto find_vmembs() const { + auto find_vmembs() const noexcept { std::vector> found; for (auto it = membs.cbegin(); it != membs.cend();) { it = std::find_if(it, membs.cend(), [](std::shared_ptr memb) { @@ -164,7 +165,7 @@ struct Cls { return found; } - auto find_pmembs() const { + auto find_pmembs() const noexcept { std::vector> found; for (auto it = membs.cbegin(); it != membs.cend();) { it = std::find_if(it, membs.cend(), [](std::shared_ptr memb) { @@ -175,7 +176,7 @@ struct Cls { return found; } - auto find_pattrs() const { + auto find_pattrs() const noexcept { std::vector> found; for (auto it = attrs.cbegin(); it != attrs.cend();) { it = std::find_if(it, attrs.cend(), [](std::shared_ptr attr) { @@ -216,7 +217,7 @@ struct Header { Header(const std::string& name) : name{name}, proper_name{get_proper_name(name)} {} - std::shared_ptr find_cls(const std::string& name) { + std::shared_ptr find_cls(const std::string& name) noexcept { std::string jname = just_name(name); auto it = std::find_if( classes.begin(), classes.end(), @@ -224,7 +225,7 @@ struct Header { return it != classes.end() ? *it : nullptr; } - auto find_overloads(const std::string& name) { + auto find_overloads(const std::string& name) noexcept { std::vector> found; for (auto it = funcs.begin(); it != funcs.end();) { it = std::find_if(it, funcs.end(), @@ -236,7 +237,7 @@ struct Header { return found; } - static std::string get_proper_name(std::string name) { + static std::string get_proper_name(std::string name) noexcept { std::replace(name.begin(), name.end(), '/', '_'); return name; } diff --git a/builder/src/utils.hpp b/builder/src/utils.hpp index 036a97a..d509e1a 100644 --- a/builder/src/utils.hpp +++ b/builder/src/utils.hpp @@ -32,6 +32,8 @@ inline CppCompoundPtr parse_file(CppParser& parser, const std::string& filename); /// @brief wrapper to get a string repr of a CppObj inline std::string str_of_cpp(const CppObj* cppObj); +/// @brief wrapper to get a string repr of a CppObj +inline std::string str_of_cpp(const CppObjPtr& cppObjPtr); /// @brief std::string to UPPER inline std::string to_upper(std::string_view str); /// @brief std::string_view to PascalCase diff --git a/builder/src/utils.hxx b/builder/src/utils.hxx index 45f9840..11af040 100644 --- a/builder/src/utils.hxx +++ b/builder/src/utils.hxx @@ -1,6 +1,7 @@ #pragma once // hpp +#include "cppast.h" #include "utils.hpp" // STL @@ -97,6 +98,10 @@ inline std::string str_of_cpp(const CppObj* cppObj) { return str; } +inline std::string str_of_cpp(const CppObjPtr& cppObjPtr) { + return str_of_cpp(cppObjPtr.get()); +} + inline std::string to_upper(std::string str) { std::transform(str.cbegin(), str.cend(), str.begin(), [](unsigned char c) { return std::toupper(c); }); diff --git a/builder/src/utils/sep.hpp b/builder/src/utils/sep.hpp index e0e34e3..e96d6f5 100644 --- a/builder/src/utils/sep.hpp +++ b/builder/src/utils/sep.hpp @@ -3,15 +3,17 @@ // STL #include -/// @brief wrapper over a `const T&` and a `const U&`, display all elements of a -/// `T` with a 'U' as a separator +/// @brief wrapper over a `const Iterable&`, a `const Separator&` and a +/// `Projection`, display all elements of a `Iterable` projected by a +/// `Projection` with a 'Separator' as a separator /// @example std::cout << Sep{std::vector{1, 2, 3}, ", "} << std::endl; /// // output: `1, 2, 3` -template +template class Sep; /// @brief ostream writer for `Sep` -template -std::ostream& operator<<(std::ostream& os, const Sep& sep); +template +inline std::ostream& operator<<( + std::ostream& os, const Sep& sep); // hxx #include "sep.hxx" diff --git a/builder/src/utils/sep.hxx b/builder/src/utils/sep.hxx index 8289e41..60d1b3d 100644 --- a/builder/src/utils/sep.hxx +++ b/builder/src/utils/sep.hxx @@ -3,24 +3,50 @@ // hpp #include "sep.hpp" -template +// STL +#include +#include +#include + +struct identity { + using is_transparent = std::true_type; + + template + constexpr T&& operator()(T&& t) const noexcept { + return std::forward(t); + } +}; + +template class Sep { public: - Sep(const T& iterable, const U& separator) - : iterable_{iterable}, separator_{separator} {} + inline Sep(const Iterable& iterable, + const Separator& separator) noexcept(noexcept(Projection{})) + : iterable_{iterable}, separator_{separator}, projection_{} {} + + inline Sep(const Iterable& iterable, const Separator& separator, + Projection projection) noexcept(noexcept(Projection{ + std::move(std::declval())})) + : iterable_{iterable}, + separator_{separator}, + projection_{std::move(projection)} {} - friend std::ostream& operator<< <>(std::ostream&, const Sep&); + friend std::ostream& operator<< <>( + std::ostream&, const Sep&); private: - const T& iterable_; - const U& separator_; + const Iterable& iterable_; + const Separator& separator_; + Projection projection_; }; -template -std::ostream& operator<<(std::ostream& os, const Sep& sep) { +template +inline std::ostream& operator<<( + std::ostream& os, const Sep& sep) { auto it = std::begin(sep.iterable_); if (it == std::end(sep.iterable_)) return os; - os << *it; - for (++it; it != std::end(sep.iterable_); ++it) os << sep.separator_ << *it; + os << sep.projection_(*it); + for (++it; it != std::end(sep.iterable_); ++it) + os << sep.separator_ << sep.projection_(*it); return os; } diff --git a/builder/src/write.hxx b/builder/src/write.hxx index 6298bff..b77dbb3 100644 --- a/builder/src/write.hxx +++ b/builder/src/write.hxx @@ -1,6 +1,8 @@ #pragma once // hpp +#include + #include "write.hpp" /// @brief output a `Cls` to write an hpp @@ -65,14 +67,18 @@ inline std::ostream& operator<<(std::ostream& os, const Cls& cls) { for (const Ctor& ctor : ctors) os << "\n .def(py::init<" << Sep{ctor.args, ", "} << ">())"; for (const Memb& memb : ptr_iter(cls.membs)) { - if (memb.is_overloaded) + if (memb.is_overloaded) { + std::vector args_names{memb.args_names}; + for (std::size_t i = 0; i < args_names.size(); ++i) + if (memb.args_type[i].find("std::unique_ptr") != std::string::npos) + args_names[i] = "std::move(" + memb.args_names[i] + ")"; os << "\n .def(\"" << memb.name << "\", [](" << (memb.is_public ? (cls.is_outsider ? cls.complete_name : cls.name) : cls.pub_name) << "& py_self" << (memb.args.empty() ? "" : ", ") << Sep{memb.args, ", "} << ") { return py_self." << memb.name << '(' - << Sep{memb.args_names, ", "} << "); })"; - else + << Sep{args_names, ", "} << "); })"; + } else os << "\n .def(\"" << memb.name << "\", &" << (memb.is_public ? (cls.is_outsider ? cls.complete_name : cls.name) : cls.pub_name) diff --git a/tests/hardware_interface/test_resource_manager.py b/tests/hardware_interface/test_resource_manager.py new file mode 100644 index 0000000..e4d5541 --- /dev/null +++ b/tests/hardware_interface/test_resource_manager.py @@ -0,0 +1,1774 @@ +from ros2_control_py import ros2_control_test_assets +from ros2_control_py.ros2_control_test_assets import ( + TEST_ACTUATOR_HARDWARE_CLASS_TYPE, + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + TEST_ACTUATOR_HARDWARE_NAME, + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + TEST_ACTUATOR_HARDWARE_TYPE, + TEST_SENSOR_HARDWARE_CLASS_TYPE, + TEST_SENSOR_HARDWARE_COMMAND_INTERFACES, + TEST_SENSOR_HARDWARE_NAME, + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + TEST_SENSOR_HARDWARE_TYPE, + TEST_SYSTEM_HARDWARE_CLASS_TYPE, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + TEST_SYSTEM_HARDWARE_NAME, + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + TEST_SYSTEM_HARDWARE_TYPE, +) +from ros2_control_py.hardware_interface import ( + ResourceManager, + ActuatorInterface, + StateInterface, + CommandInterface, + HardwareInfo, + UNCONFIGURED, + ACTIVE, + INACTIVE, + FINALIZED, + return_type, + State, + VectorString, + Time, + Duration, + FloatRef, +) +from lifecycle_msgs import msg as lifecycle_msgs_msg +import pytest +from os import getenv + +ROS_DISTRO = getenv("ROS_DISTRO") +PLUGIN_NAME = ( + "CLASS_TYPE" if hasattr(HardwareInfo, "hardware_class_type") else "PLUGIN_NAME" +) + + +def ASSERT_FALSE(a): + assert not a + + +def EXPECT_FALSE(a): + ASSERT_FALSE(a) + + +def ASSERT_TRUE(a): + assert a + + +def EXPECT_TRUE(a): + ASSERT_TRUE(a) + + +def ASSERT_EQ(a, b): + assert a == b + + +def EXPECT_EQ(a, b): + ASSERT_EQ(a, b) + + +def ASSERT_NE(a, b): + assert a != b + + +def EXPECT_NE(a, b): + ASSERT_NE(a, b) + + +class TestableResourceManager(ResourceManager): + __test__ = False + + def __init__( + self, + urdf: str = None, + validate_interfaces: bool = True, + activate_all: bool = False, + ): + if urdf is None: + super().__init__() + else: + super().__init__(urdf, validate_interfaces, activate_all) + + +def set_components_state( + rm: TestableResourceManager, + components: VectorString, + state_id: int, + state_name: str, +) -> list[return_type]: + if len(components) == 0: + components = VectorString( + ["TestActuatorHardware", "TestSensorHardware", "TestSystemHardware"] + ) + results = [] + for component in components: + state = State(state_id, state_name) + result = rm.set_component_state(component, state) + results.append(result) + return results + + +def configure_components(rm: TestableResourceManager, components: VectorString = None): + if components is None: + components = [] + return set_components_state( + rm, components, lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, INACTIVE + ) + + +def activate_components(rm: TestableResourceManager, components: VectorString = None): + if components is None: + components = [] + return set_components_state( + rm, components, lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, ACTIVE + ) + + +def deactivate_components(rm: TestableResourceManager, components: VectorString = None): + if components is None: + components = [] + return set_components_state( + rm, components, lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, INACTIVE + ) + + +def cleanup_components(rm: TestableResourceManager, components: VectorString = None): + if components is None: + components = [] + return set_components_state( + rm, + components, + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + UNCONFIGURED, + ) + + +def shutdown_components(rm: TestableResourceManager, components: VectorString = None): + if components is None: + components = [] + return set_components_state( + rm, components, lifecycle_msgs_msg.State.PRIMARY_STATE_FINALIZED, FINALIZED + ) + + +def test_initialization_empty(): + with pytest.raises(RuntimeError): + TestableResourceManager("") + + +def test_initialization_with_urdf(): + TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + + +def test_post_initialization_with_urdf(): + TestableResourceManager().load_urdf(ros2_control_test_assets.minimal_robot_urdf) + + +def test_initialization_with_urdf_manual_validation(): + # we validate the results manually + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf, False) + + EXPECT_EQ(1, rm.actuator_components_size()) + EXPECT_EQ(1, rm.sensor_components_size()) + EXPECT_EQ(1, rm.system_components_size()) + + state_interface_keys = rm.state_interface_keys() + ASSERT_EQ(11, len(state_interface_keys)) + EXPECT_TRUE(rm.state_interface_exists("joint1/position")) + EXPECT_TRUE(rm.state_interface_exists("joint1/velocity")) + EXPECT_TRUE(rm.state_interface_exists("sensor1/velocity")) + EXPECT_TRUE(rm.state_interface_exists("joint2/position")) + EXPECT_TRUE(rm.state_interface_exists("joint3/position")) + + command_interface_keys = rm.command_interface_keys() + ASSERT_EQ(6, len(command_interface_keys)) + EXPECT_TRUE(rm.command_interface_exists("joint1/position")) + EXPECT_TRUE(rm.command_interface_exists("joint2/velocity")) + EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")) + + +def test_initialization_with_wrong_urdf(): + # missing state keys + with pytest.raises(Exception): + TestableResourceManager( + ros2_control_test_assets.minimal_robot_missing_state_keys_urdf + ) + # missing command keys + with pytest.raises(Exception): + TestableResourceManager( + ros2_control_test_assets.minimal_robot_missing_command_keys_urdf + ) + + +def test_initialization_with_urdf_unclaimed(): + # we validate the results manually + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + + command_interface_keys = rm.command_interface_keys() + for key in command_interface_keys: + EXPECT_FALSE(rm.command_interface_is_claimed(key)) + # state interfaces don't have to be locked, hence any arbitrary key + # should return False. + state_interface_keys = rm.state_interface_keys() + for key in state_interface_keys: + EXPECT_FALSE(rm.command_interface_is_claimed(key)) + + +def test_no_load_urdf_function_called(): + if ROS_DISTRO != "rolling": + return + rm = TestableResourceManager() + ASSERT_FALSE(rm.is_urdf_already_loaded()) + + +def test_load_urdf_called_if_urdf_is_invalid(): + if ROS_DISTRO != "rolling": + return + rm = TestableResourceManager() + with pytest.raises(Exception): + rm.load_urdf(ros2_control_test_assets.minimal_robot_missing_state_keys_urdf) + ASSERT_TRUE(rm.is_urdf_already_loaded()) + + +def test_load_urdf_called_if_urdf_is_valid(): + if ROS_DISTRO != "rolling": + return + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + ASSERT_TRUE(rm.is_urdf_already_loaded()) + + +def test_can_load_urdf_later(): + if ROS_DISTRO != "rolling": + return + rm = TestableResourceManager() + ASSERT_FALSE(rm.is_urdf_already_loaded()) + rm.load_urdf(ros2_control_test_assets.minimal_robot_urdf) + ASSERT_TRUE(rm.is_urdf_already_loaded()) + + +def test_resource_claiming(): + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + # Activate components to get all interfaces available + activate_components(rm) + + key = "joint1/position" + EXPECT_TRUE(rm.command_interface_is_available(key)) + EXPECT_FALSE(rm.command_interface_is_claimed(key)) + + position_command_interface = rm.claim_command_interface(key) + EXPECT_TRUE(rm.command_interface_is_available(key)) + EXPECT_TRUE(rm.command_interface_is_claimed(key)) + with pytest.raises(RuntimeError): + rm.claim_command_interface(key) + EXPECT_TRUE(rm.command_interface_is_available(key)) + del position_command_interface + EXPECT_TRUE(rm.command_interface_is_available(key)) + EXPECT_FALSE(rm.command_interface_is_claimed(key)) + + # command interfaces can only be claimed once + for key in ( + "joint1/position", + "joint1/position", + "joint1/position", + "joint2/velocity", + "joint3/velocity", + ): + interface = rm.claim_command_interface(key) + EXPECT_TRUE(rm.command_interface_is_available(key)) + EXPECT_TRUE(rm.command_interface_is_claimed(key)) + with pytest.raises(RuntimeError): + rm.claim_command_interface(key) + EXPECT_TRUE(rm.command_interface_is_available(key)) + del interface + EXPECT_TRUE(rm.command_interface_is_available(key)) + EXPECT_FALSE(rm.command_interface_is_claimed(key)) + + # TODO(destogl): This claim test is not True.... can not be... + # state interfaces can be claimed multiple times + for key in [ + "joint1/position", + "joint1/velocity", + "sensor1/velocity", + "joint2/position", + "joint3/position", + ]: + EXPECT_TRUE(rm.state_interface_is_available(key)) + interface = rm.claim_state_interface(key) + EXPECT_TRUE(rm.state_interface_is_available(key)) + rm.claim_state_interface(key) + del interface + + interfaces = [] + interface_names = ["joint1/position", "joint2/velocity", "joint3/velocity"] + for key in interface_names: + EXPECT_TRUE(rm.command_interface_is_available(key)) + interfaces.append(rm.claim_command_interface(key)) + for key in interface_names: + EXPECT_TRUE(rm.command_interface_is_available(key)) + EXPECT_TRUE(rm.command_interface_is_claimed(key)) + interfaces.clear() + for key in interface_names: + EXPECT_TRUE(rm.command_interface_is_available(key)) + EXPECT_FALSE(rm.command_interface_is_claimed(key)) + + +class ExternalComponent(ActuatorInterface): + def __init__(self): + super().__init__() + + def export_state_interfaces(self) -> list[StateInterface]: + return [ + StateInterface("external_joint", "external_state_interface"), + ] + + def export_command_interfaces(self) -> list[CommandInterface]: + return [ + CommandInterface("external_joint", "external_command_interface"), + ] + + def get_name(self) -> str: + return "ExternalComponent" + + def read(self, time: Time, period: Duration) -> return_type: + return return_type.OK + + def write(self, time: Time, period: Duration) -> return_type: + return return_type.OK + + +def test_post_initialization_add_components(): + # we validate the results manually + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf, False) + # Activate components to get all interfaces available + activate_components(rm) + + EXPECT_EQ(1, rm.actuator_components_size()) + EXPECT_EQ(1, rm.sensor_components_size()) + EXPECT_EQ(1, rm.system_components_size()) + + ASSERT_EQ(11, len(rm.state_interface_keys())) + ASSERT_EQ(6, len(rm.command_interface_keys())) + + external_component_hw_info = HardwareInfo() + external_component_hw_info.name = "ExternalComponent" + external_component_hw_info.type = "actuator" + if hasattr(HardwareInfo, "is_async"): + external_component_hw_info.is_async = False + rm.import_component(ExternalComponent(), external_component_hw_info) + EXPECT_EQ(2, rm.actuator_components_size()) + + ASSERT_EQ(12, len(rm.state_interface_keys())) + EXPECT_TRUE(rm.state_interface_exists("external_joint/external_state_interface")) + ASSERT_EQ(7, len(rm.command_interface_keys())) + EXPECT_TRUE( + rm.command_interface_exists("external_joint/external_command_interface") + ) + + status_map = rm.get_components_status() + EXPECT_EQ( + status_map["ExternalComponent"].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + + configure_components(rm, ["ExternalComponent"]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map["ExternalComponent"].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + + activate_components(rm, ["ExternalComponent"]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map["ExternalComponent"].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + + rm.claim_state_interface("external_joint/external_state_interface") + rm.claim_command_interface("external_joint/external_command_interface") + + +def test_default_prepare_perform_switch(): + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + # Activate components to get all interfaces available + activate_components(rm) + + EXPECT_TRUE(rm.prepare_command_mode_switch(VectorString([""]), VectorString([""]))) + EXPECT_TRUE(rm.perform_command_mode_switch(VectorString([""]), VectorString([""]))) + + +hardware_resources_command_modes = """ + + + test_hardware_components/TestSystemCommandModes + + + + + + + + + + + + + + +""" + +command_mode_urdf = ( + ros2_control_test_assets.urdf_head + + hardware_resources_command_modes + + ros2_control_test_assets.urdf_tail +) + + +def test_custom_prepare_perform_switch(): + rm = TestableResourceManager(command_mode_urdf) + # Scenarios defined by example criteria + empty_keys = VectorString([]) + irrelevant_keys = VectorString(["elbow_joint/position", "should_joint/position"]) + illegal_single_key = VectorString(["joint1/position"]) + legal_keys_position = VectorString(["joint1/position", "joint2/position"]) + legal_keys_velocity = VectorString(["joint1/velocity", "joint2/velocity"]) + # Default behavior for empty key lists + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, empty_keys)) + + # Default behavior when given irrelevant keys + EXPECT_TRUE(rm.prepare_command_mode_switch(irrelevant_keys, irrelevant_keys)) + EXPECT_TRUE(rm.prepare_command_mode_switch(irrelevant_keys, empty_keys)) + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, irrelevant_keys)) + + # The test hardware interface has a criteria that both joints must change mode + EXPECT_FALSE(rm.prepare_command_mode_switch(illegal_single_key, illegal_single_key)) + EXPECT_FALSE(rm.prepare_command_mode_switch(illegal_single_key, empty_keys)) + EXPECT_FALSE(rm.prepare_command_mode_switch(empty_keys, illegal_single_key)) + + # Test legal start keys + EXPECT_TRUE( + rm.prepare_command_mode_switch(legal_keys_position, legal_keys_position) + ) + EXPECT_TRUE( + rm.prepare_command_mode_switch(legal_keys_velocity, legal_keys_velocity) + ) + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_position, empty_keys)) + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_position)) + EXPECT_TRUE(rm.prepare_command_mode_switch(legal_keys_velocity, empty_keys)) + EXPECT_TRUE(rm.prepare_command_mode_switch(empty_keys, legal_keys_velocity)) + + # Test rejection from perform_command_mode_switch, test hardware rejects empty start sets + EXPECT_TRUE( + rm.perform_command_mode_switch(legal_keys_position, legal_keys_position) + ) + EXPECT_FALSE(rm.perform_command_mode_switch(empty_keys, empty_keys)) + EXPECT_FALSE(rm.perform_command_mode_switch(empty_keys, legal_keys_position)) + + +def test_resource_status(): + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + + status_map = rm.get_components_status() + + # name + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].name, TEST_ACTUATOR_HARDWARE_NAME) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].name, TEST_SENSOR_HARDWARE_NAME) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].name, TEST_SYSTEM_HARDWARE_NAME) + # type + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].type, TEST_ACTUATOR_HARDWARE_TYPE) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].type, TEST_SENSOR_HARDWARE_TYPE) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].type, TEST_SYSTEM_HARDWARE_TYPE) + # plugin_name + EXPECT_EQ( + getattr(status_map[TEST_ACTUATOR_HARDWARE_NAME], PLUGIN_NAME.lower()), + globals()["TEST_ACTUATOR_HARDWARE_" + PLUGIN_NAME], + ) + EXPECT_EQ( + getattr(status_map[TEST_SENSOR_HARDWARE_NAME], PLUGIN_NAME.lower()), + globals()["TEST_SENSOR_HARDWARE_" + PLUGIN_NAME], + ) + EXPECT_EQ( + getattr(status_map[TEST_SYSTEM_HARDWARE_NAME], PLUGIN_NAME.lower()), + globals()["TEST_SYSTEM_HARDWARE_" + PLUGIN_NAME], + ) + # state + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), UNCONFIGURED) + + def check_interfaces( + registered_interfaces: VectorString, interface_names: VectorString + ): + for interface in interface_names: + EXPECT_TRUE(interface in registered_interfaces) + + check_interfaces( + status_map[TEST_ACTUATOR_HARDWARE_NAME].command_interfaces, + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + ) + EXPECT_EQ(len(status_map[TEST_SENSOR_HARDWARE_NAME].command_interfaces), 0) + check_interfaces( + status_map[TEST_SYSTEM_HARDWARE_NAME].command_interfaces, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + ) + + check_interfaces( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces, + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + ) + EXPECT_TRUE( + "joint1/some_unlisted_interface" + in status_map[TEST_ACTUATOR_HARDWARE_NAME].state_interfaces + ) + check_interfaces( + status_map[TEST_SENSOR_HARDWARE_NAME].state_interfaces, + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + ) + check_interfaces( + status_map[TEST_SYSTEM_HARDWARE_NAME].state_interfaces, + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + ) + + +def test_lifecycle_all_resources(): + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + + # All resources start as UNCONFIGURED + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), UNCONFIGURED) + del status_map + + ASSERT_TRUE(all(c == return_type.OK for c in configure_components(rm))) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), INACTIVE) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), INACTIVE) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), INACTIVE) + del status_map + + ASSERT_TRUE(all(c == return_type.OK for c in activate_components(rm))) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), ACTIVE) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), ACTIVE) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), ACTIVE) + del status_map + + ASSERT_TRUE(all(c == return_type.OK for c in deactivate_components(rm))) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), INACTIVE) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), INACTIVE) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), INACTIVE) + del status_map + + ASSERT_TRUE(all(c == return_type.OK for c in cleanup_components(rm))) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), UNCONFIGURED) + del status_map + + ASSERT_TRUE(all(c == return_type.OK for c in shutdown_components(rm))) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_FINALIZED, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), FINALIZED) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_FINALIZED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), FINALIZED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_FINALIZED, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), FINALIZED) + del status_map + + +def test_lifecycle_individual_resources(): + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + + # All resources start as UNCONFIGURED + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), UNCONFIGURED) + del status_map + + configure_components(rm, [TEST_ACTUATOR_HARDWARE_NAME]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), INACTIVE) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), UNCONFIGURED) + del status_map + + activate_components(rm, [TEST_ACTUATOR_HARDWARE_NAME]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), ACTIVE) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), UNCONFIGURED) + del status_map + + configure_components(rm, [TEST_SENSOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), ACTIVE) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), INACTIVE) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), INACTIVE) + del status_map + + activate_components(rm, [TEST_SENSOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), ACTIVE) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), ACTIVE) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), ACTIVE) + del status_map + + deactivate_components(rm, [TEST_ACTUATOR_HARDWARE_NAME, TEST_SENSOR_HARDWARE_NAME]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), INACTIVE) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), INACTIVE) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), ACTIVE) + del status_map + + cleanup_components(rm, [TEST_SENSOR_HARDWARE_NAME]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), INACTIVE) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), ACTIVE) + del status_map + + shutdown_components(rm, [TEST_ACTUATOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_FINALIZED, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), FINALIZED) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), UNCONFIGURED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_FINALIZED, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), FINALIZED) + del status_map + + shutdown_components(rm, [TEST_SENSOR_HARDWARE_NAME]) + status_map = rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_FINALIZED, + ) + EXPECT_EQ(status_map[TEST_ACTUATOR_HARDWARE_NAME].state.label(), FINALIZED) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_FINALIZED, + ) + EXPECT_EQ(status_map[TEST_SENSOR_HARDWARE_NAME].state.label(), FINALIZED) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_FINALIZED, + ) + EXPECT_EQ(status_map[TEST_SYSTEM_HARDWARE_NAME].state.label(), FINALIZED) + del status_map + + +def test_resource_availability_and_claiming_in_lifecycle(): + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + + def check_interfaces( + interface_names: VectorString, check_method, expected_result: bool + ): + for interface in interface_names: + EXPECT_EQ(check_method(interface), expected_result) + + def check_interface_claiming( + state_interface_names: VectorString, + command_interface_names: VectorString, + expected_result: bool, + ): + states = [] + commands = [] + + if expected_result: + for key in state_interface_names: + states.append(rm.claim_state_interface(key)) + for key in command_interface_names: + commands.append(rm.claim_command_interface(key)) + else: + for key in state_interface_names: + with pytest.raises(RuntimeError): + states.append(rm.claim_state_interface(key)) + for key in command_interface_names: + with pytest.raises(RuntimeError): + commands.append(rm.claim_command_interface(key)) + + check_interfaces( + command_interface_names, + lambda arg1: rm.command_interface_is_claimed(arg1), + expected_result, + ) + + # All resources start as UNCONFIGURED - All interfaces are imported but not available + check_interfaces( + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_exists(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_exists(arg1), + True, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_exists(arg1), + True, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_exists(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_exists(arg1), + True, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + False, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + False, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + False, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + False, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + False, + ) + + # Nothing can be claimed + check_interface_claiming( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + False, + ) + check_interface_claiming(TEST_SENSOR_HARDWARE_STATE_INTERFACES, [], False) + check_interface_claiming( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + False, + ) + + # When actuator is configured all interfaces become available + configure_components(rm, {TEST_ACTUATOR_HARDWARE_NAME}) + check_interfaces( + ["joint1/position"], lambda arg1: rm.command_interface_is_available(arg1), True + ) + check_interfaces( + ["joint1/max_velocity"], + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + False, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + False, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + False, + ) + + # Can claim Actuator's interfaces + check_interface_claiming([], ["joint1/position"], True) + check_interface_claiming( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, ["joint1/max_velocity"], True + ) + check_interface_claiming(TEST_SENSOR_HARDWARE_STATE_INTERFACES, [], False) + check_interface_claiming( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + False, + ) + + # When actuator is activated all state- and command- interfaces become available + activate_components(rm, {TEST_ACTUATOR_HARDWARE_NAME}) + check_interfaces( + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + False, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + False, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + False, + ) + + # Can claim all Actuator's state interfaces and command interfaces + check_interface_claiming( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + True, + ) + check_interface_claiming(TEST_SENSOR_HARDWARE_STATE_INTERFACES, [], False) + check_interface_claiming( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + False, + ) + + # Check if all interfaces still exits + check_interfaces( + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_exists(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_exists(arg1), + True, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_exists(arg1), + True, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_exists(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_exists(arg1), + True, + ) + + # When Sensor and System are configured their state- + # and command- interfaces are available + configure_components(rm, [TEST_SENSOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME]) + check_interfaces( + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + check_interfaces( + ["joint2/velocity", "joint3/velocity"], + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + check_interfaces( + ["joint2/max_acceleration", "configuration/max_tcp_jerk"], + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + + # Can claim: + # - all Actuator's state interfaces and command interfaces + # - sensor's state interfaces + # - system's state and command interfaces + check_interface_claiming( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + True, + ) + check_interface_claiming(TEST_SENSOR_HARDWARE_STATE_INTERFACES, [], True) + check_interface_claiming([], ["joint2/velocity", "joint3/velocity"], True) + check_interface_claiming( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + ["joint2/max_acceleration", "configuration/max_tcp_jerk"], + True, + ) + + # All active - everything available + activate_components(rm, [TEST_SENSOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME]) + check_interfaces( + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + + # Can claim everything + # - actuator's state interfaces and command interfaces + # - sensor's state interfaces + # - system's state and non-moving command interfaces + check_interface_claiming( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + True, + ) + check_interface_claiming(TEST_SENSOR_HARDWARE_STATE_INTERFACES, [], True) + check_interface_claiming( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + True, + ) + + # When deactivated - movement interfaces are not available anymore + deactivate_components(rm, [TEST_ACTUATOR_HARDWARE_NAME, TEST_SENSOR_HARDWARE_NAME]) + check_interfaces( + ["joint1/position"], lambda arg1: rm.command_interface_is_available(arg1), True + ) + check_interfaces( + ["joint1/max_velocity"], + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + + # Can claim everything + # - actuator's state and command interfaces + # - sensor's state interfaces + # - system's state and command interfaces + check_interface_claiming([], ["joint1/position"], True) + check_interface_claiming( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, ["joint1/max_velocity"], True + ) + check_interface_claiming(TEST_SENSOR_HARDWARE_STATE_INTERFACES, [], True) + check_interface_claiming( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + True, + ) + + # When sensor is cleaned up the interfaces are not available anymore + cleanup_components(rm, {TEST_SENSOR_HARDWARE_NAME}) + check_interfaces( + ["joint1/position"], lambda arg1: rm.command_interface_is_available(arg1), True + ) + check_interfaces( + ["joint1/max_velocity"], + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_is_available(arg1), + True, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + False, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_is_available(arg1), + True, + ) + + # Can claim everything + # - actuator's state and command interfaces + # - no sensor's interface + # - system's state and command interfaces + check_interface_claiming([], ["joint1/position"], True) + check_interface_claiming( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, ["joint1/max_velocity"], True + ) + check_interface_claiming(TEST_SENSOR_HARDWARE_STATE_INTERFACES, [], False) + check_interface_claiming( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + True, + ) + + # Check if all interfaces still exits + check_interfaces( + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_exists(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES, + lambda arg1: rm.command_interface_exists(arg1), + True, + ) + + check_interfaces( + TEST_ACTUATOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_exists(arg1), + True, + ) + check_interfaces( + TEST_SENSOR_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_exists(arg1), + True, + ) + check_interfaces( + TEST_SYSTEM_HARDWARE_STATE_INTERFACES, + lambda arg1: rm.state_interface_exists(arg1), + True, + ) + + +def test_managing_controllers_reference_interfaces(): + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf) + + CONTROLLER_NAME = "test_controller" + REFERENCE_INTERFACE_NAMES = VectorString(["input1", "input2", "input3"]) + FULL_REFERENCE_INTERFACE_NAMES = VectorString( + [ + CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[0], + CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[1], + CONTROLLER_NAME + "/" + REFERENCE_INTERFACE_NAMES[2], + ] + ) + + reference_interfaces = [] + reference_interface_values = [FloatRef(1.0), FloatRef(2.0), FloatRef(3.0)] + + for name, value in zip(REFERENCE_INTERFACE_NAMES, reference_interface_values): + reference_interfaces.append(CommandInterface(CONTROLLER_NAME, name, value)) + + rm.import_controller_reference_interfaces(CONTROLLER_NAME, reference_interfaces) + + ASSERT_EQ( + rm.get_controller_reference_interface_names(CONTROLLER_NAME), + FULL_REFERENCE_INTERFACE_NAMES, + ) + + # check that all interfaces are imported properly + for interface in FULL_REFERENCE_INTERFACE_NAMES: + EXPECT_TRUE(rm.command_interface_exists(interface)) + EXPECT_FALSE(rm.command_interface_is_available(interface)) + EXPECT_FALSE(rm.command_interface_is_claimed(interface)) + + # make interface available + rm.make_controller_reference_interfaces_available(CONTROLLER_NAME) + for interface in FULL_REFERENCE_INTERFACE_NAMES: + EXPECT_TRUE(rm.command_interface_exists(interface)) + EXPECT_TRUE(rm.command_interface_is_available(interface)) + EXPECT_FALSE(rm.command_interface_is_claimed(interface)) + + # try to make interfaces available from unknown controller + with pytest.raises(IndexError): + rm.make_controller_reference_interfaces_available("unknown_controller") + + # claim interfaces in a scope that deletes them after + claimed_itf1 = rm.claim_command_interface(FULL_REFERENCE_INTERFACE_NAMES[0]) + claimed_itf3 = rm.claim_command_interface(FULL_REFERENCE_INTERFACE_NAMES[2]) + + for interface in FULL_REFERENCE_INTERFACE_NAMES: + EXPECT_TRUE(rm.command_interface_exists(interface)) + EXPECT_TRUE(rm.command_interface_is_available(interface)) + EXPECT_TRUE(rm.command_interface_is_claimed(FULL_REFERENCE_INTERFACE_NAMES[0])) + EXPECT_FALSE(rm.command_interface_is_claimed(FULL_REFERENCE_INTERFACE_NAMES[1])) + EXPECT_TRUE(rm.command_interface_is_claimed(FULL_REFERENCE_INTERFACE_NAMES[2])) + + # access interface value + EXPECT_EQ(claimed_itf1.get_value(), 1.0) + EXPECT_EQ(claimed_itf3.get_value(), 3.0) + + claimed_itf1.set_value(11.1) + claimed_itf3.set_value(33.3) + EXPECT_EQ(claimed_itf1.get_value(), 11.1) + EXPECT_EQ(claimed_itf3.get_value(), 33.3) + + EXPECT_EQ(reference_interface_values[0], 11.1) + EXPECT_EQ(reference_interface_values[1], 2.0) + EXPECT_EQ(reference_interface_values[2], 33.3) + + del claimed_itf1 + del claimed_itf3 + + # interfaces should be released now, but still managed by resource manager + for interface in FULL_REFERENCE_INTERFACE_NAMES: + EXPECT_TRUE(rm.command_interface_exists(interface)) + EXPECT_TRUE(rm.command_interface_is_available(interface)) + EXPECT_FALSE(rm.command_interface_is_claimed(interface)) + + # make interfaces unavailable + rm.make_controller_reference_interfaces_unavailable(CONTROLLER_NAME) + for interface in FULL_REFERENCE_INTERFACE_NAMES: + EXPECT_TRUE(rm.command_interface_exists(interface)) + EXPECT_FALSE(rm.command_interface_is_available(interface)) + EXPECT_FALSE(rm.command_interface_is_claimed(interface)) + + # try to make interfaces unavailable from unknown controller + with pytest.raises(IndexError): + rm.make_controller_reference_interfaces_unavailable("unknown_controller") + + # Last written values should stay + EXPECT_EQ(reference_interface_values[0], 11.1) + EXPECT_EQ(reference_interface_values[1], 2.0) + EXPECT_EQ(reference_interface_values[2], 33.3) + + # remove reference interfaces from resource manager + rm.remove_controller_reference_interfaces(CONTROLLER_NAME) + + # they should not exist in resource manager + for interface in FULL_REFERENCE_INTERFACE_NAMES: + EXPECT_FALSE(rm.command_interface_exists(interface)) + EXPECT_FALSE(rm.command_interface_is_available(interface)) + + # try to remove interfaces from unknown controller + with pytest.raises(IndexError): + rm.make_controller_reference_interfaces_unavailable("unknown_controller") + + +class TestResourceManagerReadWriteError: + def setup_method(self, method): + self.rm = TestableResourceManager() + self.claimed_itfs = [] + self.time = Time(0) + self.duration = Duration(0, 10000000) + # values to set to hardware to simulate failure on read and write + + def setup_resource_manager_and_do_initial_checks(self): + self.rm = TestableResourceManager( + ros2_control_test_assets.minimal_robot_urdf, False + ) + activate_components(self.rm) + + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + + self.claimed_itfs.append( + self.rm.claim_command_interface( + TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0] + ) + ) + self.claimed_itfs.append( + self.rm.claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0]) + ) + + self.check_if_interface_available(True, True) + # with default values read and write should run without any problems + ok, failed_hardware_names = self.rm.read(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + ok, failed_hardware_names = self.rm.write(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + self.check_if_interface_available(True, True) + + # check if all interfaces are available + def check_if_interface_available( + self, actuator_interfaces: bool, system_interfaces: bool + ): + for interface in TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES: + EXPECT_EQ( + self.rm.command_interface_is_available(interface), actuator_interfaces + ) + for interface in TEST_ACTUATOR_HARDWARE_STATE_INTERFACES: + EXPECT_EQ( + self.rm.state_interface_is_available(interface), actuator_interfaces + ) + for interface in TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES: + EXPECT_EQ( + self.rm.command_interface_is_available(interface), system_interfaces + ) + for interface in TEST_SYSTEM_HARDWARE_STATE_INTERFACES: + EXPECT_EQ( + self.rm.state_interface_is_available(interface), system_interfaces + ) + + def check_read_or_write_failure( + self, method_that_fails, other_method, fail_value: bool + ): + # define state to set components to + state_active = State(lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, ACTIVE) + + # read failure for TEST_ACTUATOR_HARDWARE_NAME + self.claimed_itfs[0].set_value(fail_value) + self.claimed_itfs[1].set_value(fail_value - 10.0) + ok, failed_hardware_names = method_that_fails(self.time, self.duration) + EXPECT_FALSE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + ASSERT_EQ(failed_hardware_names, VectorString([TEST_ACTUATOR_HARDWARE_NAME])) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + self.check_if_interface_available(False, True) + self.rm.set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + self.check_if_interface_available(True, True) + # write is sill OK + ok, failed_hardware_names = other_method(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + self.check_if_interface_available(True, True) + + # read failure for TEST_SYSTEM_HARDWARE_NAME + self.claimed_itfs[0].set_value(fail_value - 10.0) + self.claimed_itfs[1].set_value(fail_value) + ok, failed_hardware_names = method_that_fails(self.time, self.duration) + EXPECT_FALSE(ok) + EXPECT_NE(len(failed_hardware_names), 0) + ASSERT_EQ(failed_hardware_names, VectorString([TEST_SYSTEM_HARDWARE_NAME])) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + self.check_if_interface_available(True, False) + self.rm.set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + self.check_if_interface_available(True, True) + # write is sill OK + ok, failed_hardware_names = other_method(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + self.check_if_interface_available(True, True) + + # read failure for both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + self.claimed_itfs[0].set_value(fail_value) + self.claimed_itfs[1].set_value(fail_value) + ok, failed_hardware_names = method_that_fails(self.time, self.duration) + EXPECT_FALSE(ok) + EXPECT_NE(len(failed_hardware_names), 0) + ASSERT_EQ( + failed_hardware_names, + VectorString([TEST_ACTUATOR_HARDWARE_NAME, TEST_SYSTEM_HARDWARE_NAME]), + ) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_UNCONFIGURED, + ) + self.check_if_interface_available(False, False) + self.rm.set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active) + self.rm.set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + self.check_if_interface_available(True, True) + # write is sill OK + ok, failed_hardware_names = other_method(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + self.check_if_interface_available(True, True) + + def check_read_or_write_deactivate( + self, method_that_deactivates, other_method, deactivate_value: bool + ): + # define state to set components to + state_active = State(lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, ACTIVE) + + # deactivate for TEST_ACTUATOR_HARDWARE_NAME + self.claimed_itfs[0].set_value(deactivate_value) + self.claimed_itfs[1].set_value(deactivate_value - 10.0) + # deactivate on error + ok, failed_hardware_names = method_that_deactivates(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + self.check_if_interface_available(True, True) + + # reactivate + self.rm.set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + self.check_if_interface_available(True, True) + del status_map + # write is sill OK + ok, failed_hardware_names = other_method(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + self.check_if_interface_available(True, True) + + # deactivate for TEST_SYSTEM_HARDWARE_NAME + self.claimed_itfs[0].set_value(deactivate_value - 10.0) + self.claimed_itfs[1].set_value(deactivate_value) + # deactivate on flag + ok, failed_hardware_names = method_that_deactivates(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + self.check_if_interface_available(True, True) + # re-activate + self.rm.set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + self.check_if_interface_available(True, True) + del status_map + # write is sill OK + ok, failed_hardware_names = other_method(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + self.check_if_interface_available(True, True) + + # deactivate both, TEST_ACTUATOR_HARDWARE_NAME and TEST_SYSTEM_HARDWARE_NAME + self.claimed_itfs[0].set_value(deactivate_value) + self.claimed_itfs[1].set_value(deactivate_value) + # deactivate on flag + ok, failed_hardware_names = method_that_deactivates(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_INACTIVE, + ) + self.check_if_interface_available(True, True) + # re-activate + self.rm.set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_active) + self.rm.set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_active) + status_map = self.rm.get_components_status() + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs_msg.State.PRIMARY_STATE_ACTIVE, + ) + self.check_if_interface_available(True, True) + del status_map + # write is sill OK + ok, failed_hardware_names = other_method(self.time, self.duration) + EXPECT_TRUE(ok) + EXPECT_EQ(len(failed_hardware_names), 0) + self.check_if_interface_available(True, True) + + def test_handle_error_on_hardware_read(self): + if ROS_DISTRO != "rolling": + return + self.setup_resource_manager_and_do_initial_checks() + + # check read methods failures + self.check_read_or_write_failure( + lambda time, duration: rm.read(time, duration), + lambda time, duration: rm.write(time, duration), + test_constants.READ_FAIL_VALUE, + ) + + def test_handle_error_on_hardware_write(self): + if ROS_DISTRO != "rolling": + return + self.setup_resource_manager_and_do_initial_checks() + + # check write methods failures + self.check_read_or_write_failure( + lambda time, duration: rm.write(time, duration), + lambda time, duration: rm.read(time, duration), + test_constants.WRITE_FAIL_VALUE, + ) + + def test_handle_deactivate_on_hardware_read(self): + if ROS_DISTRO != "rolling": + return + self.setup_resource_manager_and_do_initial_checks() + + # check read methods failures + self.check_read_or_write_deactivate( + lambda time, duration: rm.read(time, duration), + lambda time, duration: rm.write(time, duration), + test_constants.READ_DEACTIVATE_VALUE, + ) + + def test_handle_deactivate_on_hardware_write(self): + if ROS_DISTRO != "rolling": + return + self.setup_resource_manager_and_do_initial_checks() + + # check write methods failures + self.check_read_or_write_deactivate( + lambda time, duration: rm.write(time, duration), + lambda time, duration: rm.read(time, duration), + test_constants.WRITE_DEACTIVATE_VALUE, + ) + + +def test_test_caching_of_controllers_to_hardware(): + if ROS_DISTRO != "rolling": + return + rm = TestableResourceManager(ros2_control_test_assets.minimal_robot_urdf, False) + activate_components(rm) + + TEST_CONTROLLER_ACTUATOR_NAME = "test_controller_actuator" + TEST_CONTROLLER_SYSTEM_NAME = "test_controller_system" + TEST_BROADCASTER_ALL_NAME = "test_broadcaster_all" + TEST_BROADCASTER_SENSOR_NAME = "test_broadcaster_sensor" + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_ACTUATOR_NAME, TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES + ) + rm.cache_controller_to_hardware( + TEST_BROADCASTER_ALL_NAME, TEST_ACTUATOR_HARDWARE_STATE_INTERFACES + ) + + rm.cache_controller_to_hardware( + TEST_CONTROLLER_SYSTEM_NAME, TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES + ) + rm.cache_controller_to_hardware( + TEST_BROADCASTER_ALL_NAME, TEST_SYSTEM_HARDWARE_STATE_INTERFACES + ) + + rm.cache_controller_to_hardware( + TEST_BROADCASTER_SENSOR_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES + ) + rm.cache_controller_to_hardware( + TEST_BROADCASTER_ALL_NAME, TEST_SENSOR_HARDWARE_STATE_INTERFACES + ) + + controllers = rm.get_cached_controllers_to_hardware(TEST_ACTUATOR_HARDWARE_NAME) + ASSERT_EQ( + controllers, + VectorString([TEST_CONTROLLER_ACTUATOR_NAME, TEST_BROADCASTER_ALL_NAME]), + ) + + controllers = rm.get_cached_controllers_to_hardware(TEST_SYSTEM_HARDWARE_NAME) + ASSERT_EQ( + controllers, + VectorString([TEST_CONTROLLER_SYSTEM_NAME, TEST_BROADCASTER_ALL_NAME]), + ) + + controllers = rm.get_cached_controllers_to_hardware(TEST_SENSOR_HARDWARE_NAME) + ASSERT_EQ( + controllers, + VectorString([TEST_BROADCASTER_SENSOR_NAME, TEST_BROADCASTER_ALL_NAME]), + )