diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e267856eb1..262abec840 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -171,6 +171,20 @@ if(BUILD_TESTING) DESTINATION lib ) + add_library(test_controller_failed_activate SHARED + test/test_controller_failed_activate/test_controller_failed_activate.cpp + ) + target_link_libraries(test_controller_failed_activate PUBLIC + controller_manager + ) + target_compile_definitions(test_controller_failed_activate PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file( + controller_interface test/test_controller_failed_activate/test_controller_failed_activate.xml) + install( + TARGETS test_controller_failed_activate + DESTINATION lib + ) + ament_add_gmock(test_release_interfaces test/test_release_interfaces.cpp APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ diff --git a/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp new file mode 100644 index 0000000000..c13df1a713 --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp @@ -0,0 +1,66 @@ +// Copyright 2021 Department of Engineering Cybernetics, NTNU. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_controller_failed_activate.hpp" + +#include +#include + +#include "lifecycle_msgs/msg/transition.hpp" + +namespace test_controller_failed_activate +{ +TestControllerFailedActivate::TestControllerFailedActivate() +: controller_interface::ControllerInterface() +{ +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_init() +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type TestControllerFailedActivate::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_configure(const rclcpp_lifecycle::State & /*previous_state&*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_activate(const rclcpp_lifecycle::State & /*previous_state&*/) +{ + // Simply simulate a controller that can not be activated + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +} // namespace test_controller_failed_activate + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + test_controller_failed_activate::TestControllerFailedActivate, + controller_interface::ControllerInterface) diff --git a/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp new file mode 100644 index 0000000000..483d2b0158 --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp @@ -0,0 +1,75 @@ +// Copyright 2020 Department of Engineering Cybernetics, NTNU +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ +#define TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ + +#include +#include + +#include "controller_interface/visibility_control.h" +#include "controller_manager/controller_manager.hpp" + +namespace test_controller_failed_activate +{ +// Corresponds to the name listed within the pluginglib xml +constexpr char TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME[] = + "controller_manager/test_controller_failed_activate"; +// Corresponds to the command interface to claim +constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint2/velocity"; +class TestControllerFailedActivate : public controller_interface::ControllerInterface +{ +public: + CONTROLLER_MANAGER_PUBLIC + TestControllerFailedActivate(); + + CONTROLLER_MANAGER_PUBLIC + virtual ~TestControllerFailedActivate() = default; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::INDIVIDUAL, + {TEST_CONTROLLER_COMMAND_INTERFACE}}; + } + + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + CONTROLLER_MANAGER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; +}; + +} // namespace test_controller_failed_activate + +#endif // TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ diff --git a/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml new file mode 100644 index 0000000000..80c4e6bef5 --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml @@ -0,0 +1,9 @@ + + + + + Controller used for testing + + + + diff --git a/hardware_interface_testing/CMakeLists.txt b/hardware_interface_testing/CMakeLists.txt index cd8e30028a..838607a239 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/hardware_interface_testing/CMakeLists.txt @@ -21,12 +21,14 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() add_library(test_components SHARED -test/test_components/test_actuator.cpp -test/test_components/test_sensor.cpp -test/test_components/test_system.cpp) + test/test_components/test_actuator.cpp + test/test_components/test_sensor.cpp + test/test_components/test_system.cpp + test/test_components/test_actuator_exclusive_interfaces.cpp +) ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets) install(TARGETS test_components -DESTINATION lib + DESTINATION lib ) pluginlib_export_plugin_description_file( hardware_interface test/test_components/test_components.xml) diff --git a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp new file mode 100644 index 0000000000..acc7d214c6 --- /dev/null +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp @@ -0,0 +1,173 @@ +// Copyright 2020 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" + +using hardware_interface::ActuatorInterface; +using hardware_interface::CommandInterface; +using hardware_interface::return_type; +using hardware_interface::StateInterface; + +static std::pair extract_joint_and_interface( + const std::string & full_name) +{ + // Signature is: interface/joint + const auto joint_name = full_name.substr(0, full_name.find_last_of('/')); + const auto interface_name = full_name.substr(full_name.find_last_of('/') + 1); + + return {joint_name, interface_name}; +} +struct JointState +{ + double pos; + double vel; + double effort; +}; + +class TestActuatorExclusiveInterfaces : public ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if (ActuatorInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + for (const auto & j : info.joints) + { + (void)j; // Suppress unused warning + current_states_.emplace_back(JointState{}); + } + + return CallbackReturn::SUCCESS; + } + + std::vector export_state_interfaces() override + { + std::vector state_interfaces; + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { + const auto & joint = info_.joints[i]; + + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, hardware_interface::HW_IF_POSITION, ¤t_states_.at(i).pos)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, hardware_interface::HW_IF_VELOCITY, ¤t_states_.at(i).vel)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, hardware_interface::HW_IF_EFFORT, ¤t_states_.at(i).effort)); + } + return state_interfaces; + } + + std::vector export_command_interfaces() override + { + std::vector command_interfaces; + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { + const auto & joint = info_.joints[i]; + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_POSITION, ¤t_states_.at(i).pos)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_VELOCITY, ¤t_states_.at(i).vel)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_EFFORT, ¤t_states_.at(i).effort)); + } + return command_interfaces; + } + + hardware_interface::return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override + { + std::vector claimed_joint_copy = currently_claimed_joints_; + + for (const auto & interface : stop_interfaces) + { + const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); + if ( + interface_name == hardware_interface::HW_IF_POSITION || + interface_name == hardware_interface::HW_IF_VELOCITY || + interface_name == hardware_interface::HW_IF_EFFORT) + { + claimed_joint_copy.erase( + std::remove(claimed_joint_copy.begin(), claimed_joint_copy.end(), joint_name), + claimed_joint_copy.end()); + } + } + + for (const auto & interface : start_interfaces) + { + const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); + if ( + interface_name == hardware_interface::HW_IF_POSITION || + interface_name == hardware_interface::HW_IF_VELOCITY || + interface_name == hardware_interface::HW_IF_EFFORT) + { + if ( + std::find(claimed_joint_copy.begin(), claimed_joint_copy.end(), joint_name) != + claimed_joint_copy.end()) + { + return hardware_interface::return_type::ERROR; + } + } + } + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override + { + for (const auto & interface : stop_interfaces) + { + const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); + currently_claimed_joints_.erase( + std::remove(currently_claimed_joints_.begin(), currently_claimed_joints_.end(), joint_name), + currently_claimed_joints_.end()); + } + + for (const auto & interface : start_interfaces) + { + const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); + + // TODO make sure it is not possible to claim multiple interfaces of the same axis + currently_claimed_joints_.push_back(joint_name); + } + + return hardware_interface::return_type::OK; + } + + return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return return_type::OK; + } + + return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return return_type::OK; + } + +private: + std::vector currently_claimed_joints_; + std::vector current_states_; +}; + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(TestActuatorExclusiveInterfaces, hardware_interface::ActuatorInterface) diff --git a/hardware_interface_testing/test/test_components/test_components.xml b/hardware_interface_testing/test/test_components/test_components.xml index e24ee28317..fdcd0ea5ee 100644 --- a/hardware_interface_testing/test/test_components/test_components.xml +++ b/hardware_interface_testing/test/test_components/test_components.xml @@ -5,6 +5,12 @@ Test Actuator + + + Test Actuator + + +