test: move to start example controller unit tests

franka_example_controllers/CMakeLists.txt

@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.5)
 project(franka_example_controllers)
 
-# Default to C++14
+# Default to C++17
 if(NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 17)
     set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -81,18 +81,32 @@ if(BUILD_TESTING)
     ament_flake8()
     ament_pep257()
     ament_xmllint()
-    ament_add_gmock(${PROJECT_NAME}_test test/test_load_gravity_compensation_controller.cpp)
+    ament_add_gmock(${PROJECT_NAME}_test
+        test/test_load_gravity_compensation_controller.cpp
+    )
     target_include_directories(${PROJECT_NAME}_test PRIVATE include)
-
     ament_target_dependencies(${PROJECT_NAME}_test
         controller_manager
         ros2_control_test_assets
     )
 
+    ament_add_gmock(${PROJECT_NAME}_test_load_move_to_start
+        test/test_load_move_to_start_example_controller.cpp)
+    target_include_directories(${PROJECT_NAME}_test_load_move_to_start PRIVATE include)
+    ament_target_dependencies(${PROJECT_NAME}_test_load_move_to_start
+        controller_manager
+        ros2_control_test_assets
+    )
+
+
     ament_add_gmock(${PROJECT_NAME}_gravity_test test/test_gravity_compensation_example.cpp)
     target_include_directories(${PROJECT_NAME}_gravity_test PRIVATE include)
     target_link_libraries(${PROJECT_NAME}_gravity_test ${PROJECT_NAME})
 
+    ament_add_gmock(${PROJECT_NAME}_move_to_start_test test/test_move_to_start_example_controller.cpp)
+    target_include_directories(${PROJECT_NAME}_move_to_start_test PRIVATE include)
+    target_link_libraries(${PROJECT_NAME}_move_to_start_test ${PROJECT_NAME})
+
     if(CHECK_TIDY)
         find_package(ament_cmake_clang_tidy REQUIRED)
         set(ament_cmake_clang_tidy_CONFIG_FILE ../.clang-tidy)

franka_example_controllers/test/test_load_move_to_start_example_controller.cpp

@@ -0,0 +1,39 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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 <gtest/gtest.h>
+#include <memory>
+
+#include "controller_manager/controller_manager.hpp"
+#include "hardware_interface/resource_manager.hpp"
+#include "rclcpp/executors/single_threaded_executor.hpp"
+#include "ros2_control_test_assets/descriptions.hpp"
+
+TEST(TestLoadMoveToStartExampleController, load_controller) {
+  rclcpp::init(0, nullptr);
+
+  std::shared_ptr<rclcpp::Executor> executor =
+      std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+
+  controller_manager::ControllerManager cm(std::make_unique<hardware_interface::ResourceManager>(
+                                               ros2_control_test_assets::minimal_robot_urdf),
+                                           executor, "test_controller_manager");
+
+  auto response = cm.load_controller("test_move_to_start_example_controller",
+                                     "franka_example_controllers/MoveToStartExampleController");
+
+  ASSERT_NE(response, nullptr);
+
+  rclcpp::shutdown();
+}

franka_example_controllers/test/test_move_to_start_example_controller.cpp

@@ -0,0 +1,137 @@
+// Copyright (c) 2023 Franka Emika GmbH
+//
+// 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 <memory>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "test_move_to_start_example_controller.hpp"
+
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+
+const double k_EPS = 1e-5;
+
+void MoveToStartExampleControllerTest::SetUpTestSuite() {
+  rclcpp::init(0, nullptr);
+}
+
+void MoveToStartExampleControllerTest::TearDownTestSuite() {
+  rclcpp::shutdown();
+}
+
+void MoveToStartExampleControllerTest::SetUp() {
+  controller_ = std::make_unique<franka_example_controllers::MoveToStartExampleController>();
+}
+
+void MoveToStartExampleControllerTest::TearDown() {
+  controller_.reset(nullptr);
+}
+
+void MoveToStartExampleControllerTest::SetUpController() {
+  const auto result = controller_->init("test_move_to_start_example");
+  ASSERT_EQ(result, controller_interface::return_type::OK);
+  std::vector<LoanedCommandInterface> command_ifs;
+  std::vector<LoanedStateInterface> state_ifs;
+
+  command_ifs.emplace_back(joint_1_pos_cmd_);
+  command_ifs.emplace_back(joint_2_pos_cmd_);
+  command_ifs.emplace_back(joint_3_pos_cmd_);
+  command_ifs.emplace_back(joint_4_pos_cmd_);
+  command_ifs.emplace_back(joint_5_pos_cmd_);
+  command_ifs.emplace_back(joint_6_pos_cmd_);
+  command_ifs.emplace_back(joint_7_pos_cmd_);
+
+  state_ifs.emplace_back(joint_1_pos_state_);
+  state_ifs.emplace_back(joint_1_vel_state_);
+  state_ifs.emplace_back(joint_2_pos_state_);
+  state_ifs.emplace_back(joint_2_vel_state_);
+  state_ifs.emplace_back(joint_3_pos_state_);
+  state_ifs.emplace_back(joint_3_vel_state_);
+  state_ifs.emplace_back(joint_4_pos_state_);
+  state_ifs.emplace_back(joint_4_vel_state_);
+  state_ifs.emplace_back(joint_5_pos_state_);
+  state_ifs.emplace_back(joint_5_vel_state_);
+  state_ifs.emplace_back(joint_6_pos_state_);
+  state_ifs.emplace_back(joint_6_vel_state_);
+  state_ifs.emplace_back(joint_7_pos_state_);
+  state_ifs.emplace_back(joint_7_vel_state_);
+
+  controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs));
+}
+
+TEST_F(MoveToStartExampleControllerTest, controller_gains_not_set_failure) {
+  SetUpController();
+
+  // Failure due to not set K, D gains
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::FAILURE);
+}
+
+TEST_F(MoveToStartExampleControllerTest, contoller_gain_empty) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", std::vector<double>()});
+
+  // Failure due empty k gain parameter
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::FAILURE);
+}
+
+TEST_F(MoveToStartExampleControllerTest, contoller_damping_gain_empty) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", K_gains_});
+  controller_->get_node()->set_parameter({"d_gains", std::vector<double>()});
+
+  // Failure due to empty d gain parameter
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::FAILURE);
+}
+
+TEST_F(MoveToStartExampleControllerTest, correct_controller_gains_success) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", K_gains_});
+  controller_->get_node()->set_parameter({"d_gains", D_gains_});
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+}
+
+TEST_F(MoveToStartExampleControllerTest, correct_setup_on_activate_expect_success) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", K_gains_});
+  controller_->get_node()->set_parameter({"d_gains", D_gains_});
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+}
+
+TEST_F(MoveToStartExampleControllerTest, correct_setup_on_update_expect_ok) {
+  SetUpController();
+  controller_->get_node()->set_parameter({"k_gains", K_gains_});
+  controller_->get_node()->set_parameter({"d_gains", D_gains_});
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+
+  auto time = rclcpp::Time(0);
+  auto duration = rclcpp::Duration(0, 0);
+
+  ASSERT_EQ(controller_->update(time, duration), controller_interface::return_type::OK);
+
+  EXPECT_NEAR(joint_1_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_2_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_3_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_4_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_5_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_6_pos_cmd_.get_value(), 0.0, k_EPS);
+  EXPECT_NEAR(joint_7_pos_cmd_.get_value(), 0.0, k_EPS);
+}

franka_example_controllers/test/test_move_to_start_example_controller.hpp

@@ -0,0 +1,85 @@
+// Copyright 2023 Franka Emika GmbH.
+//
+// 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.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "gtest/gtest.h"
+
+#include "franka_example_controllers/move_to_start_example_controller.hpp"
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+
+using hardware_interface::CommandInterface;
+using hardware_interface::HW_IF_EFFORT;
+using hardware_interface::HW_IF_POSITION;
+using hardware_interface::HW_IF_VELOCITY;
+using hardware_interface::LoanedCommandInterface;
+using hardware_interface::LoanedStateInterface;
+using hardware_interface::StateInterface;
+
+class MoveToStartExampleControllerTest : public ::testing::Test {
+ public:
+  static void SetUpTestSuite();
+  static void TearDownTestSuite();
+
+  void SetUp();
+  void TearDown();
+
+  void SetUpController();
+
+ protected:
+  std::unique_ptr<franka_example_controllers::MoveToStartExampleController> controller_;
+
+  // dummy joint state values used for tests
+  const std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3", "joint4",
+                                                 "joint5", "joint6", "joint7"};
+  std::vector<double> joint_commands_ = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+
+  // The joint goal pose for the move to start controller
+  std::vector<double> joint_q_state_ = {0, -0.785398, 0, -2.35619, 0, 1.5708, 0.785398};
+  std::vector<double> joint_dq_state_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+
+  std::vector<double> K_gains_ = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+  std::vector<double> D_gains_ = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+
+  CommandInterface joint_1_pos_cmd_{joint_names_[0], HW_IF_EFFORT, &joint_commands_[0]};
+  CommandInterface joint_2_pos_cmd_{joint_names_[1], HW_IF_EFFORT, &joint_commands_[1]};
+  CommandInterface joint_3_pos_cmd_{joint_names_[2], HW_IF_EFFORT, &joint_commands_[2]};
+  CommandInterface joint_4_pos_cmd_{joint_names_[3], HW_IF_EFFORT, &joint_commands_[3]};
+  CommandInterface joint_5_pos_cmd_{joint_names_[4], HW_IF_EFFORT, &joint_commands_[4]};
+  CommandInterface joint_6_pos_cmd_{joint_names_[5], HW_IF_EFFORT, &joint_commands_[5]};
+  CommandInterface joint_7_pos_cmd_{joint_names_[6], HW_IF_EFFORT, &joint_commands_[6]};
+
+  // Position state interfaces
+  StateInterface joint_1_pos_state_{joint_names_[0], HW_IF_POSITION, &joint_q_state_[0]};
+  StateInterface joint_2_pos_state_{joint_names_[1], HW_IF_POSITION, &joint_q_state_[1]};
+  StateInterface joint_3_pos_state_{joint_names_[2], HW_IF_POSITION, &joint_q_state_[2]};
+  StateInterface joint_4_pos_state_{joint_names_[3], HW_IF_POSITION, &joint_q_state_[3]};
+  StateInterface joint_5_pos_state_{joint_names_[4], HW_IF_POSITION, &joint_q_state_[4]};
+  StateInterface joint_6_pos_state_{joint_names_[5], HW_IF_POSITION, &joint_q_state_[5]};
+  StateInterface joint_7_pos_state_{joint_names_[6], HW_IF_POSITION, &joint_q_state_[6]};
+
+  // Velocity state interfaces
+  StateInterface joint_1_vel_state_{joint_names_[0], HW_IF_VELOCITY, &joint_dq_state_[0]};
+  StateInterface joint_2_vel_state_{joint_names_[1], HW_IF_VELOCITY, &joint_dq_state_[1]};
+  StateInterface joint_3_vel_state_{joint_names_[2], HW_IF_VELOCITY, &joint_dq_state_[2]};
+  StateInterface joint_4_vel_state_{joint_names_[3], HW_IF_VELOCITY, &joint_dq_state_[3]};
+  StateInterface joint_5_vel_state_{joint_names_[4], HW_IF_VELOCITY, &joint_dq_state_[4]};
+  StateInterface joint_6_vel_state_{joint_names_[5], HW_IF_VELOCITY, &joint_dq_state_[5]};
+  StateInterface joint_7_vel_state_{joint_names_[6], HW_IF_VELOCITY, &joint_dq_state_[6]};
+};