Skip to content

Commit

Permalink
[JTC] Extend tests (ros-controls#612)
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed Jul 17, 2023
1 parent 76b1ce7 commit 16c6289
Show file tree
Hide file tree
Showing 3 changed files with 958 additions and 948 deletions.
153 changes: 123 additions & 30 deletions joint_trajectory_controller/test/test_trajectory_actions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -197,7 +197,24 @@ class TestTrajectoryActions : public TrajectoryControllerTest
}
};

TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal)
// From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest
class TestTrajectoryActionsTestParameterized
: public TestTrajectoryActions,
public ::testing::WithParamInterface<
std::tuple<std::vector<std::string>, std::vector<std::string>>>
{
public:
virtual void SetUp()
{
TestTrajectoryActions::SetUp();
command_interface_types_ = std::get<0>(GetParam());
state_interface_types_ = std::get<1>(GetParam());
}

static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); }
};

TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal)
{
SetUpExecutor();
SetUpControllerHardware();
Expand All @@ -223,12 +240,15 @@ TEST_F(TestTrajectoryActions, test_success_single_point_sendgoal)
EXPECT_TRUE(gh_future.get());
EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);

EXPECT_EQ(1.0, joint_pos_[0]);
EXPECT_EQ(2.0, joint_pos_[1]);
EXPECT_EQ(3.0, joint_pos_[2]);
if (traj_controller_->has_position_command_interface())
{
EXPECT_EQ(1.0, joint_pos_[0]);
EXPECT_EQ(2.0, joint_pos_[1]);
EXPECT_EQ(3.0, joint_pos_[2]);
}
}

TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal)
TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal)
{
SetUpExecutor();
SetUpControllerHardware();
Expand Down Expand Up @@ -270,11 +290,18 @@ TEST_F(TestTrajectoryActions, test_success_multi_point_sendgoal)
EXPECT_TRUE(gh_future.get());
EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_);

EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD);
if (traj_controller_->has_position_command_interface())
{
EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD);
}
}

/**
* Makes sense with position command interface only,
* because no integration to position state interface is implemented
*/
TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success)
{
// set tolerance parameters
Expand Down Expand Up @@ -308,11 +335,18 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success)
EXPECT_EQ(
control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_);

EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD);
if (traj_controller_->has_position_command_interface())
{
EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD);
}
}

/**
* Makes sense with position command interface only,
* because no integration to position state interface is implemented
*/
TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success)
{
// set tolerance parameters
Expand Down Expand Up @@ -363,12 +397,15 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success)
EXPECT_EQ(
control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_);

EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD);
if (traj_controller_->has_position_command_interface())
{
EXPECT_NEAR(7.0, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(8.0, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(9.0, joint_pos_[2], COMMON_THRESHOLD);
}
}

TEST_F(TestTrajectoryActions, test_state_tolerances_fail)
TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail)
{
// set joint tolerance parameters
const double state_tol = 0.0001;
Expand Down Expand Up @@ -415,12 +452,15 @@ TEST_F(TestTrajectoryActions, test_state_tolerances_fail)
// run an update, it should be holding
updateController(rclcpp::Duration::from_seconds(0.01));

EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD);
if (traj_controller_->has_position_command_interface())
{
EXPECT_NEAR(INITIAL_POS_JOINT1, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(INITIAL_POS_JOINT2, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(INITIAL_POS_JOINT3, joint_pos_[2], COMMON_THRESHOLD);
}
}

TEST_F(TestTrajectoryActions, test_goal_tolerances_fail)
TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail)
{
// set joint tolerance parameters
const double goal_tol = 0.1;
Expand Down Expand Up @@ -465,12 +505,15 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_fail)
// run an update, it should be holding
updateController(rclcpp::Duration::from_seconds(0.01));

EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD);
if (traj_controller_->has_position_command_interface())
{
EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD);
}
}

TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail)
TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail)
{
// set joint tolerance parameters
const double state_tol = 0.0001;
Expand Down Expand Up @@ -512,12 +555,15 @@ TEST_F(TestTrajectoryActions, test_no_time_from_start_state_tolerance_fail)
// run an update, it should be holding
updateController(rclcpp::Duration::from_seconds(0.01));

EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD);
if (traj_controller_->has_position_command_interface())
{
EXPECT_NEAR(init_pos1, joint_pos_[0], COMMON_THRESHOLD);
EXPECT_NEAR(init_pos2, joint_pos_[1], COMMON_THRESHOLD);
EXPECT_NEAR(init_pos3, joint_pos_[2], COMMON_THRESHOLD);
}
}

TEST_F(TestTrajectoryActions, test_cancel_hold_position)
TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position)
{
SetUpExecutor();
SetUpControllerHardware();
Expand Down Expand Up @@ -561,7 +607,54 @@ TEST_F(TestTrajectoryActions, test_cancel_hold_position)
// run an update, it should be holding
updateController(rclcpp::Duration::from_seconds(0.01));

EXPECT_EQ(prev_pos1, joint_pos_[0]);
EXPECT_EQ(prev_pos2, joint_pos_[1]);
EXPECT_EQ(prev_pos3, joint_pos_[2]);
if (traj_controller_->has_position_command_interface())
{
EXPECT_EQ(prev_pos1, joint_pos_[0]);
EXPECT_EQ(prev_pos2, joint_pos_[1]);
EXPECT_EQ(prev_pos3, joint_pos_[2]);
}
}

// position controllers
INSTANTIATE_TEST_SUITE_P(
PositionTrajectoryControllersActions, TestTrajectoryActionsTestParameterized,
::testing::Values(
std::make_tuple(std::vector<std::string>({"position"}), std::vector<std::string>({"position"})),
std::make_tuple(
std::vector<std::string>({"position"}), std::vector<std::string>({"position", "velocity"})),
std::make_tuple(
std::vector<std::string>({"position"}),
std::vector<std::string>({"position", "velocity", "acceleration"}))));

// position_velocity controllers
INSTANTIATE_TEST_SUITE_P(
PositionVelocityTrajectoryControllersActions, TestTrajectoryActionsTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>({"position", "velocity"}), std::vector<std::string>({"position"})),
std::make_tuple(
std::vector<std::string>({"position", "velocity"}),
std::vector<std::string>({"position", "velocity"})),
std::make_tuple(
std::vector<std::string>({"position", "velocity"}),
std::vector<std::string>({"position", "velocity", "acceleration"}))));

// only velocity controller
INSTANTIATE_TEST_SUITE_P(
OnlyVelocityTrajectoryControllersAction, TestTrajectoryActionsTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>({"velocity"}), std::vector<std::string>({"position", "velocity"})),
std::make_tuple(
std::vector<std::string>({"velocity"}),
std::vector<std::string>({"position", "velocity", "acceleration"}))));

// only effort controller
INSTANTIATE_TEST_SUITE_P(
OnlyEffortTrajectoryControllers, TestTrajectoryActionsTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>({"effort"}), std::vector<std::string>({"position", "velocity"})),
std::make_tuple(
std::vector<std::string>({"effort"}),
std::vector<std::string>({"position", "velocity", "acceleration"}))));
Loading

0 comments on commit 16c6289

Please sign in to comment.