From 09fd66679a3902cd74ad941c9a0da6ff7a7b1c93 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 8 Sep 2023 16:30:34 +0900 Subject: [PATCH] Add test for effort gripper controller (#769) (cherry picked from commit fb46a77df7a6179a7724c7ac0145b0be4740cbfa) --- .../test/test_gripper_controllers.cpp | 103 +++++++++++------- .../test/test_gripper_controllers.hpp | 10 +- 2 files changed, 67 insertions(+), 46 deletions(-) diff --git a/gripper_controllers/test/test_gripper_controllers.cpp b/gripper_controllers/test/test_gripper_controllers.cpp index 4c82eb6961..506f968b99 100644 --- a/gripper_controllers/test/test_gripper_controllers.cpp +++ b/gripper_controllers/test/test_gripper_controllers.cpp @@ -32,123 +32,142 @@ using hardware_interface::LoanedStateInterface; using GripperCommandAction = control_msgs::action::GripperCommand; using GoalHandle = rclcpp_action::ServerGoalHandle; -void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); } +template +void GripperControllerTest::SetUpTestCase() +{ + rclcpp::init(0, nullptr); +} -void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); } +template +void GripperControllerTest::TearDownTestCase() +{ + rclcpp::shutdown(); +} -void GripperControllerTest::SetUp() +template +void GripperControllerTest::SetUp() { // initialize controller - controller_ = std::make_unique(); + controller_ = std::make_unique>(); } -void GripperControllerTest::TearDown() { controller_.reset(nullptr); } +template +void GripperControllerTest::TearDown() +{ + controller_.reset(nullptr); +} -void GripperControllerTest::SetUpController() +template +void GripperControllerTest::SetUpController() { const auto result = controller_->init("gripper_controller"); ASSERT_EQ(result, controller_interface::return_type::OK); std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_); + command_ifs.emplace_back(this->joint_1_cmd_); std::vector state_ifs; - state_ifs.emplace_back(joint_1_pos_state_); - state_ifs.emplace_back(joint_1_vel_state_); + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); } -TEST_F(GripperControllerTest, ParametersNotSet) +using TestTypes = ::testing::Types< + std::integral_constant, + std::integral_constant>; +TYPED_TEST_SUITE(GripperControllerTest, TestTypes); + +TYPED_TEST(GripperControllerTest, ParametersNotSet) { - SetUpController(); + this->SetUpController(); // configure failed, 'joints' parameter not set ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::ERROR); } -TEST_F(GripperControllerTest, JointParameterIsEmpty) +TYPED_TEST(GripperControllerTest, JointParameterIsEmpty) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", ""}); + this->controller_->get_node()->set_parameter({"joint", ""}); // configure failed, 'joints' is empty ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::ERROR); } -TEST_F(GripperControllerTest, ConfigureParamsSuccess) +TYPED_TEST(GripperControllerTest, ConfigureParamsSuccess) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "joint_1"}); + this->controller_->get_node()->set_parameter({"joint", "joint_1"}); // configure successful ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } -TEST_F(GripperControllerTest, ActivateWithWrongJointsNamesFails) +TYPED_TEST(GripperControllerTest, ActivateWithWrongJointsNamesFails) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); + this->controller_->get_node()->set_parameter({"joint", "unicorn_joint"}); // activate failed, 'joint4' is not a valid joint name for the hardware ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::ERROR); } -TEST_F(GripperControllerTest, ActivateSuccess) +TYPED_TEST(GripperControllerTest, ActivateSuccess) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "joint1"}); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); // activate successful ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } -TEST_F(GripperControllerTest, ActivateDeactivateActivateSuccess) +TYPED_TEST(GripperControllerTest, ActivateDeactivateActivateSuccess) { - SetUpController(); + this->SetUpController(); - controller_->get_node()->set_parameter({"joint", "joint1"}); + this->controller_->get_node()->set_parameter({"joint", "joint1"}); ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_deactivate(rclcpp_lifecycle::State()), + this->controller_->on_deactivate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); // re-assign interfaces std::vector command_ifs; - command_ifs.emplace_back(joint_1_pos_cmd_); + command_ifs.emplace_back(this->joint_1_cmd_); std::vector state_ifs; - state_ifs.emplace_back(joint_1_pos_state_); - state_ifs.emplace_back(joint_1_vel_state_); - controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + state_ifs.emplace_back(this->joint_1_pos_state_); + state_ifs.emplace_back(this->joint_1_vel_state_); + this->controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); ASSERT_EQ( - controller_->on_configure(rclcpp_lifecycle::State()), + this->controller_->on_configure(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); ASSERT_EQ( - controller_->on_activate(rclcpp_lifecycle::State()), + this->controller_->on_activate(rclcpp_lifecycle::State()), controller_interface::CallbackReturn::SUCCESS); } diff --git a/gripper_controllers/test/test_gripper_controllers.hpp b/gripper_controllers/test/test_gripper_controllers.hpp index 350e551cb8..4983c8102d 100644 --- a/gripper_controllers/test/test_gripper_controllers.hpp +++ b/gripper_controllers/test/test_gripper_controllers.hpp @@ -26,20 +26,22 @@ #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::StateInterface; namespace { - // subclassing and friending so we can access member variables +template class FriendGripperController -: public gripper_action_controller::GripperActionController +: public gripper_action_controller::GripperActionController { FRIEND_TEST(GripperControllerTest, CommandSuccessTest); }; +template class GripperControllerTest : public ::testing::Test { public: @@ -53,7 +55,7 @@ class GripperControllerTest : public ::testing::Test void SetUpHandles(); protected: - std::unique_ptr controller_; + std::unique_ptr> controller_; // dummy joint state values used for tests const std::string joint_name_ = "joint1"; @@ -62,7 +64,7 @@ class GripperControllerTest : public ::testing::Test StateInterface joint_1_pos_state_{joint_name_, HW_IF_POSITION, &joint_states_[0]}; StateInterface joint_1_vel_state_{joint_name_, HW_IF_VELOCITY, &joint_states_[1]}; - CommandInterface joint_1_pos_cmd_{joint_name_, HW_IF_POSITION, &joint_commands_[0]}; + CommandInterface joint_1_cmd_{joint_name_, T::value, &joint_commands_[0]}; }; } // anonymous namespace