Add tests for ControllerInterface class.

controller_interface/CMakeLists.txt

@@ -9,11 +9,17 @@ 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
+  src/chainable_controller_interface.cpp
+)
 target_include_directories(
   controller_interface
   PRIVATE
-  include)
+  include
+)
 ament_target_dependencies(
   controller_interface
   hardware_interface
@@ -34,16 +40,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)

controller_interface/package.xml

@@ -19,8 +19,6 @@
   <exec_depend>sensor_msgs</exec_depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

controller_interface/src/controller_interface.cpp

@@ -20,6 +20,7 @@
 #include <vector>
 
 #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: <controller>::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(

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 <gmock/gmock.h>
+#include <memory>
+
+#include "rclcpp/executor_options.hpp"
+#include "rclcpp/executors/multi_threaded_executor.hpp"
+
+template <class T, size_t N>
+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::executors::MultiThreadedExecutor>(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();
+}

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_