Skip to content

Commit fcc0847

Browse files
christophfroehlichchristophfroehlich
authored
[JTC] Activate checks for parameter validation (ros-controls#857)
1 parent 3555685 commit fcc0847

File tree

2 files changed

+91
-88
lines changed

2 files changed

+91
-88
lines changed

‎joint_trajectory_controller/test/test_trajectory_controller.cpp‎

Lines changed: 77 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -122,8 +122,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
122122
rclcpp::executors::MultiThreadedExecutor executor;
123123
SetUpTrajectoryController(executor);
124124

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

128128
auto cmd_interface_config = traj_controller_->command_interface_configuration();
129129
ASSERT_EQ(
@@ -133,8 +133,8 @@ TEST_P(TrajectoryControllerTestParameterized, activate)
133133
ASSERT_EQ(
134134
state_interface_config.names.size(), joint_names_.size() * state_interface_types_.size());
135135

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

139139
executor.cancel();
140140
}
@@ -194,13 +194,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
194194
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
195195

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

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

248-
ActivateTrajectoryController(false, deactivated_positions);
249-
state = traj_controller_->get_state();
245+
state = ActivateTrajectoryController(false, deactivated_positions);
250246
ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE);
251247

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

1921-
// TODO(destogl): this tests should be changed because we are using `generate_parameters_library`
1922-
// TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters)
1923-
// {
1924-
// auto set_parameter_and_check_result = [&]()
1925-
// {
1926-
// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED);
1927-
// SetParameters(); // This call is replacing the way parameters are set via launch
1928-
// traj_controller_->get_node()->configure();
1929-
// EXPECT_EQ(traj_controller_->get_state().id(), State::PRIMARY_STATE_UNCONFIGURED);
1930-
// };
1931-
//
1932-
// SetUpTrajectoryController(false);
1933-
//
1934-
// // command interfaces: empty
1935-
// command_interface_types_ = {};
1936-
// set_parameter_and_check_result();
1937-
//
1938-
// // command interfaces: bad_name
1939-
// command_interface_types_ = {"bad_name"};
1940-
// set_parameter_and_check_result();
1941-
//
1942-
// // command interfaces: effort has to be only
1943-
// command_interface_types_ = {"effort", "position"};
1944-
// set_parameter_and_check_result();
1945-
//
1946-
// // command interfaces: velocity - position not present
1947-
// command_interface_types_ = {"velocity", "acceleration"};
1948-
// set_parameter_and_check_result();
1949-
//
1950-
// // command interfaces: acceleration without position and velocity
1951-
// command_interface_types_ = {"acceleration"};
1952-
// set_parameter_and_check_result();
1953-
//
1954-
// // state interfaces: empty
1955-
// state_interface_types_ = {};
1956-
// set_parameter_and_check_result();
1957-
//
1958-
// // state interfaces: cannot not be effort
1959-
// state_interface_types_ = {"effort"};
1960-
// set_parameter_and_check_result();
1961-
//
1962-
// // state interfaces: bad name
1963-
// state_interface_types_ = {"bad_name"};
1964-
// set_parameter_and_check_result();
1965-
//
1966-
// // state interfaces: velocity - position not present
1967-
// state_interface_types_ = {"velocity"};
1968-
// set_parameter_and_check_result();
1969-
// state_interface_types_ = {"velocity", "acceleration"};
1970-
// set_parameter_and_check_result();
1971-
//
1972-
// // state interfaces: acceleration without position and velocity
1973-
// state_interface_types_ = {"acceleration"};
1974-
// set_parameter_and_check_result();
1975-
//
1976-
// // velocity-only command interface: position - velocity not present
1977-
// command_interface_types_ = {"velocity"};
1978-
// state_interface_types_ = {"position"};
1979-
// set_parameter_and_check_result();
1980-
// state_interface_types_ = {"velocity"};
1981-
// set_parameter_and_check_result();
1982-
//
1983-
// // effort-only command interface: position - velocity not present
1984-
// command_interface_types_ = {"effort"};
1985-
// state_interface_types_ = {"position"};
1986-
// set_parameter_and_check_result();
1987-
// state_interface_types_ = {"velocity"};
1988-
// set_parameter_and_check_result();
1989-
// }
1917+
/**
1918+
* @brief see if parameter validation is correct
1919+
*
1920+
* Note: generate_parameter_library validates parameters itself during on_init() method, but
1921+
* combinations of parameters are checked from JTC during on_configure()
1922+
*/
1923+
TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters)
1924+
{
1925+
// command interfaces: empty
1926+
command_interface_types_ = {};
1927+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK);
1928+
auto state = traj_controller_->get_node()->configure();
1929+
EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
1930+
1931+
// command interfaces: bad_name
1932+
command_interface_types_ = {"bad_name"};
1933+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1934+
1935+
// command interfaces: effort has to be only
1936+
command_interface_types_ = {"effort", "position"};
1937+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1938+
1939+
// command interfaces: velocity - position not present
1940+
command_interface_types_ = {"velocity", "acceleration"};
1941+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1942+
1943+
// command interfaces: acceleration without position and velocity
1944+
command_interface_types_ = {"acceleration"};
1945+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1946+
1947+
// state interfaces: empty
1948+
state_interface_types_ = {};
1949+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1950+
1951+
// state interfaces: cannot not be effort
1952+
state_interface_types_ = {"effort"};
1953+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1954+
1955+
// state interfaces: bad name
1956+
state_interface_types_ = {"bad_name"};
1957+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1958+
1959+
// state interfaces: velocity - position not present
1960+
state_interface_types_ = {"velocity"};
1961+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1962+
state_interface_types_ = {"velocity", "acceleration"};
1963+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1964+
1965+
// state interfaces: acceleration without position and velocity
1966+
state_interface_types_ = {"acceleration"};
1967+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1968+
1969+
// velocity-only command interface: position - velocity not present
1970+
command_interface_types_ = {"velocity"};
1971+
state_interface_types_ = {"position"};
1972+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK);
1973+
state = traj_controller_->get_node()->configure();
1974+
EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
1975+
state_interface_types_ = {"velocity"};
1976+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1977+
1978+
// effort-only command interface: position - velocity not present
1979+
command_interface_types_ = {"effort"};
1980+
state_interface_types_ = {"position"};
1981+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK);
1982+
state = traj_controller_->get_node()->configure();
1983+
EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED);
1984+
state_interface_types_ = {"velocity"};
1985+
EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR);
1986+
}

‎joint_trajectory_controller/test/test_trajectory_controller_utils.hpp‎

Lines changed: 14 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -188,6 +188,17 @@ class TrajectoryControllerTest : public ::testing::Test
188188

189189
void SetUpTrajectoryController(
190190
rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {})
191+
{
192+
auto ret = SetUpTrajectoryControllerLocal(parameters);
193+
if (ret != controller_interface::return_type::OK)
194+
{
195+
FAIL();
196+
}
197+
executor.add_node(traj_controller_->get_node()->get_node_base_interface());
198+
}
199+
200+
controller_interface::return_type SetUpTrajectoryControllerLocal(
201+
const std::vector<rclcpp::Parameter> & parameters = {})
191202
{
192203
traj_controller_ = std::make_shared<TestableJointTrajectoryController>();
193204

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

203-
auto ret = traj_controller_->init(controller_name_, "", 0, "", node_options);
204-
if (ret != controller_interface::return_type::OK)
205-
{
206-
FAIL();
207-
}
208-
executor.add_node(traj_controller_->get_node()->get_node_base_interface());
214+
return traj_controller_->init(controller_name_, "", 0, "", node_options);
209215
}
210216

211217
void SetPidParameters(
@@ -262,7 +268,7 @@ class TrajectoryControllerTest : public ::testing::Test
262268
initial_eff_joints);
263269
}
264270

265-
void ActivateTrajectoryController(
271+
rclcpp_lifecycle::State ActivateTrajectoryController(
266272
bool separate_cmd_and_state_values = false,
267273
const std::vector<double> initial_pos_joints = INITIAL_POS_JOINTS,
268274
const std::vector<double> initial_vel_joints = INITIAL_VEL_JOINTS,
@@ -320,7 +326,7 @@ class TrajectoryControllerTest : public ::testing::Test
320326
}
321327

322328
traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces));
323-
traj_controller_->get_node()->activate();
329+
return traj_controller_->get_node()->activate();
324330
}
325331

326332
static void TearDownTestCase() { rclcpp::shutdown(); }

0 commit comments

Comments
 (0)