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_