diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 0a0a619818..e3187672d2 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -9,11 +9,16 @@ find_package(ament_cmake REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -add_library(controller_interface SHARED src/controller_interface.cpp) +add_library( + controller_interface + SHARED + src/controller_interface.cpp +) target_include_directories( controller_interface PRIVATE - include) + include +) ament_target_dependencies( controller_interface hardware_interface @@ -34,16 +39,13 @@ install(TARGETS controller_interface if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) - find_package(ament_lint_auto REQUIRED) find_package(hardware_interface REQUIRED) find_package(sensor_msgs REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - ament_cmake_uncrustify - ament_cmake_cpplint - ) - ament_lint_auto_find_test_dependencies() + ament_add_gmock(test_controller_interface test/test_controller_interface.cpp) + target_link_libraries(test_controller_interface controller_interface) + target_include_directories(test_controller_interface PRIVATE include) ament_add_gmock(test_controller_with_options test/test_controller_with_options.cpp) target_link_libraries(test_controller_with_options controller_interface) diff --git a/controller_interface/package.xml b/controller_interface/package.xml index cbd1b53708..a3c4d09d8e 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -19,8 +19,6 @@ sensor_msgs ament_cmake_gmock - ament_lint_auto - ament_lint_common ament_cmake diff --git a/controller_interface/src/controller_interface.cpp b/controller_interface/src/controller_interface.cpp index d336b9e1a2..b9eb483278 100644 --- a/controller_interface/src/controller_interface.cpp +++ b/controller_interface/src/controller_interface.cpp @@ -20,6 +20,7 @@ #include #include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" namespace controller_interface { @@ -73,9 +74,20 @@ return_type ControllerInterface::init(const std::string & controller_name) const rclcpp_lifecycle::State & ControllerInterface::configure() { - update_rate_ = node_->get_parameter("update_rate").as_int(); + // TODO(destogl): this should actually happen in "on_configure" but I am not sure how to get + // overrides correctly in combination with std::bind. The goal is to have the following calls: + // 1. CM: controller.get_node()->configure() + // 2. LifecycleNode: ControllerInterface::on_configure() + // 3. ControllerInterface: ::on_configure() + // Then we don't need to do state-machine related checks. + // + // Other solution is to add check into the LifecycleNode if a transition is valid to trigger + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + update_rate_ = get_node()->get_parameter("update_rate").as_int(); + } - return node_->configure(); + return get_node()->configure(); } void ControllerInterface::assign_interfaces( diff --git a/controller_interface/test/test_controller_interface.cpp b/controller_interface/test/test_controller_interface.cpp new file mode 100644 index 0000000000..252d286635 --- /dev/null +++ b/controller_interface/test/test_controller_interface.cpp @@ -0,0 +1,142 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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_interface.hpp" + +#include +#include + +#include "rclcpp/executor_options.hpp" +#include "rclcpp/executors/multi_threaded_executor.hpp" + +template +constexpr size_t arrlen(T (&)[N]) +{ + return N; +} + +TEST(TestableControllerInterface, init) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + + TestableControllerInterface controller; + + // try to get node when not initialized + ASSERT_THROW(controller.get_node(), std::runtime_error); + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + ASSERT_NO_THROW(controller.get_node()); + + // update_rate is set to default 0 + ASSERT_EQ(controller.get_update_rate(), 0u); + + // Even after configure is 0 + controller.configure(); + ASSERT_EQ(controller.get_update_rate(), 0u); + + rclcpp::shutdown(); +} + +TEST(TestableControllerInterface, setting_update_rate_in_configure) +{ + // mocks the declaration of overrides parameters in a yaml file + char const * const argv[] = {"", "--ros-args", "-p", "update_rate:=2812"}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + + TestableControllerInterface controller; + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::OK); + + // initialize executor to be able to get parameter update + auto executor = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + executor->add_node(controller.get_node()->get_node_base_interface()); + auto update_executor_spin_future = + std::async(std::launch::async, [&]() -> void { executor->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + using namespace std::chrono_literals; + std::this_thread::sleep_for(50ms); + + // update_rate is set to default 0 because it is set on configure + ASSERT_EQ(controller.get_update_rate(), 0u); + + // Even after configure is 0 + controller.configure(); + ASSERT_EQ(controller.get_update_rate(), 2812u); + + // Test updating of update_rate parameter + EXPECT_EQ(std::system("ros2 param set /testable_controller_interface update_rate 623"), 0); + // Keep the same update rate until transition from 'UNCONFIGURED' TO 'INACTIVE' does not happen + controller.configure(); // No transition so the update rate should stay intact + ASSERT_NE(controller.get_update_rate(), 623u); + ASSERT_EQ(controller.get_update_rate(), 2812u); + + controller.get_node()->activate(); + controller.configure(); // No transition so the update rate should stay intact + ASSERT_NE(controller.get_update_rate(), 623u); + ASSERT_EQ(controller.get_update_rate(), 2812u); + + controller.update(controller.get_node()->now(), rclcpp::Duration::from_seconds(0.1)); + controller.configure(); // No transition so the update rate should stay intact + ASSERT_NE(controller.get_update_rate(), 623u); + ASSERT_EQ(controller.get_update_rate(), 2812u); + + controller.get_node()->deactivate(); + controller.configure(); // No transition so the update rate should stay intact + ASSERT_NE(controller.get_update_rate(), 623u); + ASSERT_EQ(controller.get_update_rate(), 2812u); + + controller.get_node()->cleanup(); + ASSERT_EQ(controller.get_update_rate(), 2812u); + // It is first changed after controller is configured again. + controller.configure(); + ASSERT_EQ(controller.get_update_rate(), 623u); + + executor->cancel(); + rclcpp::shutdown(); +} + +TEST(TestableControllerInterfaceInitError, init_with_error) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + + TestableControllerInterfaceInitError controller; + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR); + + rclcpp::shutdown(); +} + +TEST(TestableControllerInterfaceInitFailure, init_with_failure) +{ + char const * const argv[] = {""}; + int argc = arrlen(argv); + rclcpp::init(argc, argv); + + TestableControllerInterfaceInitFailure controller; + + // initialize, create node + ASSERT_EQ(controller.init(TEST_CONTROLLER_NAME), controller_interface::return_type::ERROR); + + rclcpp::shutdown(); +} diff --git a/controller_interface/test/test_controller_interface.hpp b/controller_interface/test/test_controller_interface.hpp new file mode 100644 index 0000000000..07e546fc1a --- /dev/null +++ b/controller_interface/test/test_controller_interface.hpp @@ -0,0 +1,61 @@ +// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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_INTERFACE_HPP_ +#define TEST_CONTROLLER_INTERFACE_HPP_ + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +constexpr char TEST_CONTROLLER_NAME[] = "testable_controller_interface"; + +class TestableControllerInterface : public controller_interface::ControllerInterface +{ +public: + CallbackReturn on_init() override { return CallbackReturn::SUCCESS; } + + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + controller_interface::return_type update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return controller_interface::return_type::OK; + } +}; + +class TestableControllerInterfaceInitError : public TestableControllerInterface +{ +public: + CallbackReturn on_init() override { return CallbackReturn::ERROR; } +}; + +class TestableControllerInterfaceInitFailure : public TestableControllerInterface +{ +public: + CallbackReturn on_init() override { return CallbackReturn::FAILURE; } +}; + +#endif // TEST_CONTROLLER_INTERFACE_HPP_