Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[gripper] Add test for effort gripper controller (backport #769) #867

Merged
merged 1 commit into from
Nov 30, 2023
Merged

[gripper] Add test for effort gripper controller (backport #769) #867

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
103 changes: 61 additions & 42 deletions gripper_controllers/test/test_gripper_controllers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,123 +32,142 @@ using hardware_interface::LoanedStateInterface;
using GripperCommandAction = control_msgs::action::GripperCommand;
using GoalHandle = rclcpp_action::ServerGoalHandle<GripperCommandAction>;

void GripperControllerTest::SetUpTestCase() { rclcpp::init(0, nullptr); }
template <typename T>
void GripperControllerTest<T>::SetUpTestCase()
{
rclcpp::init(0, nullptr);
}

void GripperControllerTest::TearDownTestCase() { rclcpp::shutdown(); }
template <typename T>
void GripperControllerTest<T>::TearDownTestCase()
{
rclcpp::shutdown();
}

void GripperControllerTest::SetUp()
template <typename T>
void GripperControllerTest<T>::SetUp()
{
// initialize controller
controller_ = std::make_unique<FriendGripperController>();
controller_ = std::make_unique<FriendGripperController<T::value>>();
}

void GripperControllerTest::TearDown() { controller_.reset(nullptr); }
template <typename T>
void GripperControllerTest<T>::TearDown()
{
controller_.reset(nullptr);
}

void GripperControllerTest::SetUpController()
template <typename T>
void GripperControllerTest<T>::SetUpController()
{
const auto result = controller_->init("gripper_controller");
ASSERT_EQ(result, controller_interface::return_type::OK);

std::vector<LoanedCommandInterface> command_ifs;
command_ifs.emplace_back(joint_1_pos_cmd_);
command_ifs.emplace_back(this->joint_1_cmd_);
std::vector<LoanedStateInterface> 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<const char *, HW_IF_POSITION>,
std::integral_constant<const char *, HW_IF_EFFORT>>;
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<LoanedCommandInterface> command_ifs;
command_ifs.emplace_back(joint_1_pos_cmd_);
command_ifs.emplace_back(this->joint_1_cmd_);
std::vector<LoanedStateInterface> 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);
}
10 changes: 6 additions & 4 deletions gripper_controllers/test/test_gripper_controllers.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <const char * HardwareInterface>
class FriendGripperController
: public gripper_action_controller::GripperActionController<HW_IF_POSITION>
: public gripper_action_controller::GripperActionController<HardwareInterface>
{
FRIEND_TEST(GripperControllerTest, CommandSuccessTest);
};

template <typename T>
class GripperControllerTest : public ::testing::Test
{
public:
Expand All @@ -53,7 +55,7 @@ class GripperControllerTest : public ::testing::Test
void SetUpHandles();

protected:
std::unique_ptr<FriendGripperController> controller_;
std::unique_ptr<FriendGripperController<T::value>> controller_;

// dummy joint state values used for tests
const std::string joint_name_ = "joint1";
Expand All @@ -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
Expand Down
Loading