diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index ffd93c88..e3168238 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/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) diff --git a/franka_example_controllers/test/test_load_move_to_start_example_controller.cpp b/franka_example_controllers/test/test_load_move_to_start_example_controller.cpp new file mode 100644 index 00000000..68257dda --- /dev/null +++ b/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 +#include + +#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 executor = + std::make_shared(); + + controller_manager::ControllerManager cm(std::make_unique( + 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(); +} diff --git a/franka_example_controllers/test/test_move_to_start_example_controller.cpp b/franka_example_controllers/test/test_move_to_start_example_controller.cpp new file mode 100644 index 00000000..da59d0e5 --- /dev/null +++ b/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 +#include + +#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(); +} + +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 command_ifs; + std::vector 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()}); + + // 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()}); + + // 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); +} diff --git a/franka_example_controllers/test/test_move_to_start_example_controller.hpp b/franka_example_controllers/test/test_move_to_start_example_controller.hpp new file mode 100644 index 00000000..0e6b5d99 --- /dev/null +++ b/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 +#include +#include + +#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 controller_; + + // dummy joint state values used for tests + const std::vector joint_names_ = {"joint1", "joint2", "joint3", "joint4", + "joint5", "joint6", "joint7"}; + std::vector 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 joint_q_state_ = {0, -0.785398, 0, -2.35619, 0, 1.5708, 0.785398}; + std::vector joint_dq_state_ = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + std::vector K_gains_ = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; + std::vector 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]}; +};