Skip to content

Commit

Permalink
Add test for effort gripper controller
Browse files Browse the repository at this point in the history
  • Loading branch information
@chama1176
chama1176 committed Sep 8, 2023
1 parent 4d0b999 commit 3226ad2
Show file tree
Hide file tree
Showing 2 changed files with 67 additions and 46 deletions.
103 changes: 61 additions & 42 deletions gripper_controllers/test/test_gripper_controllers.cpp
View file
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
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

0 comments on commit 3226ad2

Please sign in to comment.