Skip to content

Commit

Permalink
[JTC] Activate checks for parameter validation (#857)
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich authored Nov 30, 2023
1 parent 3555685 commit fcc0847
Show file tree
Hide file tree
Showing 2 changed files with 91 additions and 88 deletions.
157 changes: 77 additions & 80 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,8 +122,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
rclcpp::executors::MultiThreadedExecutor executor;
SetUpTrajectoryController(executor);

traj_controller_->get_node()->configure();
ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_INACTIVE);
auto state = traj_controller_->get_node()->configure();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);

auto cmd_interface_config = traj_controller_->command_interface_configuration();
ASSERT_EQ(
Expand All @@ -133,8 +133,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
ASSERT_EQ(
state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size());

ActivateTrajectoryController();
ASSERT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_ACTIVE);
state = ActivateTrajectoryController();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE);

executor.cancel();
}
Expand Down Expand Up @@ -194,13 +194,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));

// This call is replacing the way parameters are set via launch
traj_controller_->configure();
auto state = traj_controller_->get_state();
auto state = traj_controller_->configure();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());

ActivateTrajectoryController();

state = traj_controller_->get_state();
state = ActivateTrajectoryController();
ASSERT_EQ(State::PRIMARY_STATE_ACTIVE, state.id());
EXPECT_EQ(INITIAL_POS_JOINT1, joint_pos_[0]);
EXPECT_EQ(INITIAL_POS_JOINT2, joint_pos_[1]);
Expand Down Expand Up @@ -245,8 +242,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
// wait so controller would have processed the third point when reactivated -> but it shouldn't
std::this_thread::sleep_for(std::chrono::milliseconds(3000));

ActivateTrajectoryController(false, deactivated_positions);
state = traj_controller_->get_state();
state = ActivateTrajectoryController(false, deactivated_positions);
ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE);

// it should still be holding the position at time of deactivation
Expand Down Expand Up @@ -1918,72 +1914,73 @@ INSTANTIATE_TEST_SUITE_P(
std::vector<std::string>({"effort"}),
std::vector<std::string>({"position", "velocity", "acceleration"}))));

// TODO(destogl): this tests should be changed because we are using `generate_parameters_library`
// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters)
// {
// auto set_parameter_and_check_result = [&]()
// {
// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED);
// SetParameters(); // This call is replacing the way parameters are set via launch
// traj_controller_->get_node()->configure();
// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED);
// };
//
// SetUpTrajectoryController(false);
//
// // command interfaces: empty
// command_interface_types_ = {};
// set_parameter_and_check_result();
//
// // command interfaces: bad_name
// command_interface_types_ = {"bad_name"};
// set_parameter_and_check_result();
//
// // command interfaces: effort has to be only
// command_interface_types_ = {"effort", "position"};
// set_parameter_and_check_result();
//
// // command interfaces: velocity - position not present
// command_interface_types_ = {"velocity", "acceleration"};
// set_parameter_and_check_result();
//
// // command interfaces: acceleration without position and velocity
// command_interface_types_ = {"acceleration"};
// set_parameter_and_check_result();
//
// // state interfaces: empty
// state_interface_types_ = {};
// set_parameter_and_check_result();
//
// // state interfaces: cannot not be effort
// state_interface_types_ = {"effort"};
// set_parameter_and_check_result();
//
// // state interfaces: bad name
// state_interface_types_ = {"bad_name"};
// set_parameter_and_check_result();
//
// // state interfaces: velocity - position not present
// state_interface_types_ = {"velocity"};
// set_parameter_and_check_result();
// state_interface_types_ = {"velocity", "acceleration"};
// set_parameter_and_check_result();
//
// // state interfaces: acceleration without position and velocity
// state_interface_types_ = {"acceleration"};
// set_parameter_and_check_result();
//
// // velocity-only command interface: position - velocity not present
// command_interface_types_ = {"velocity"};
// state_interface_types_ = {"position"};
// set_parameter_and_check_result();
// state_interface_types_ = {"velocity"};
// set_parameter_and_check_result();
//
// // effort-only command interface: position - velocity not present
// command_interface_types_ = {"effort"};
// state_interface_types_ = {"position"};
// set_parameter_and_check_result();
// state_interface_types_ = {"velocity"};
// set_parameter_and_check_result();
// }
/**
* @brief see if parameter validation is correct
*
* Note: generate_parameter_library validates parameters itself during on_init() method, but
* combinations of parameters are checked from JTC during on_configure()
*/
TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters)
{
// command interfaces: empty
command_interface_types_ = {};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK);
auto state = traj_controller_->get_node()->configure();
EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);

// command interfaces: bad_name
command_interface_types_ = {"bad_name"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// command interfaces: effort has to be only
command_interface_types_ = {"effort", "position"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// command interfaces: velocity - position not present
command_interface_types_ = {"velocity", "acceleration"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// command interfaces: acceleration without position and velocity
command_interface_types_ = {"acceleration"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// state interfaces: empty
state_interface_types_ = {};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// state interfaces: cannot not be effort
state_interface_types_ = {"effort"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// state interfaces: bad name
state_interface_types_ = {"bad_name"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// state interfaces: velocity - position not present
state_interface_types_ = {"velocity"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
state_interface_types_ = {"velocity", "acceleration"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// state interfaces: acceleration without position and velocity
state_interface_types_ = {"acceleration"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// velocity-only command interface: position - velocity not present
command_interface_types_ = {"velocity"};
state_interface_types_ = {"position"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK);
state = traj_controller_->get_node()->configure();
EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
state_interface_types_ = {"velocity"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);

// effort-only command interface: position - velocity not present
command_interface_types_ = {"effort"};
state_interface_types_ = {"position"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK);
state = traj_controller_->get_node()->configure();
EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
state_interface_types_ = {"velocity"};
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
}
22 changes: 14 additions & 8 deletions joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -188,6 +188,17 @@ class TrajectoryControllerTest : public ::testing::Test

void SetUpTrajectoryController(
rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {})
{
auto ret = SetUpTrajectoryControllerLocal(parameters);
if (ret != controller_interface::return_type::OK)
{
FAIL();
}
executor.add_node(traj_controller_->get_node()->get_node_base_interface());
}

controller_interface::return_type SetUpTrajectoryControllerLocal(
const std::vector<rclcpp::Parameter> & parameters = {})
{
traj_controller_ = std::make_shared<TestableJointTrajectoryController>();

Expand All @@ -200,12 +211,7 @@ class TrajectoryControllerTest : public ::testing::Test
parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end());
node_options.parameter_overrides(parameter_overrides);

auto ret = traj_controller_->init(controller_name_, "", 0, "", node_options);
if (ret != controller_interface::return_type::OK)
{
FAIL();
}
executor.add_node(traj_controller_->get_node()->get_node_base_interface());
return traj_controller_->init(controller_name_, "", 0, "", node_options);
}

void SetPidParameters(
Expand Down Expand Up @@ -262,7 +268,7 @@ class TrajectoryControllerTest : public ::testing::Test
initial_eff_joints);
}

void ActivateTrajectoryController(
rclcpp_lifecycle::State ActivateTrajectoryController(
bool separate_cmd_and_state_values = false,
const std::vector<double> initial_pos_joints = INITIAL_POS_JOINTS,
const std::vector<double> initial_vel_joints = INITIAL_VEL_JOINTS,
Expand Down Expand Up @@ -320,7 +326,7 @@ class TrajectoryControllerTest : public ::testing::Test
}

traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces));
traj_controller_->get_node()->activate();
return traj_controller_->get_node()->activate();
}

static void TearDownTestCase() { rclcpp::shutdown(); }
Expand Down

0 comments on commit fcc0847

Please sign in to comment.