Skip to content

Commit

Permalink
[JTC] Activate checks for parameter validation (#857)
Browse files Browse the repository at this point in the history 
(cherry picked from commit fcc0847)

# Conflicts:
#	joint_trajectory_controller/test/test_trajectory_controller.cpp
#	joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
  • Loading branch information
@christophfroehlich @mergify
christophfroehlich authored and mergify[bot] committed Nov 30, 2023
1 parent c8f1b44 commit 958f492
Show file tree
Hide file tree
Showing 2 changed files with 118 additions and 77 deletions.
160 changes: 84 additions & 76 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -184,8 +184,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 @@ -195,8 +195,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 @@ -257,14 +257,16 @@ 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
<<<<<<< HEAD
SetParameters();
traj_controller_->configure();
auto state = traj_controller_->get_state();
=======
auto state = traj_controller_->configure();
>>>>>>> fcc0847 ([JTC] Activate checks for parameter validation (#857))
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 @@ -317,8 +319,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
// reactivated
// wait so controller process the third point when reactivated
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
<<<<<<< HEAD
ActivateTrajectoryController();
state = traj_controller_->get_state();
=======

state = ActivateTrajectoryController(false, deactivated_positions);
>>>>>>> fcc0847 ([JTC] Activate checks for parameter validation (#857))
ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE);

// TODO(christophfroehlich) add test if there is no active trajectory after
Expand Down Expand Up @@ -1692,72 +1699,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);
}
35 changes: 34 additions & 1 deletion joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,9 +155,21 @@ class TrajectoryControllerTest : public ::testing::Test
}

void SetUpTrajectoryController(rclcpp::Executor & executor, bool use_local_parameters = true)
{
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>();

<<<<<<< HEAD
if (use_local_parameters)
{
traj_controller_->set_joint_names(joint_names_);
Expand All @@ -180,6 +192,18 @@ class TrajectoryControllerTest : public ::testing::Test
const rclcpp::Parameter cmd_interfaces_params("command_interfaces", command_interface_types_);
const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_);
node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params});
=======
auto node_options = rclcpp::NodeOptions();
std::vector<rclcpp::Parameter> parameter_overrides;
parameter_overrides.push_back(rclcpp::Parameter("joints", joint_names_));
parameter_overrides.push_back(
rclcpp::Parameter("command_interfaces", command_interface_types_));
parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_));
parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end());
node_options.parameter_overrides(parameter_overrides);

return traj_controller_->init(controller_name_, "", 0, "", node_options);
>>>>>>> fcc0847 ([JTC] Activate checks for parameter validation (#857))
}

void SetPidParameters(
Expand Down Expand Up @@ -224,7 +248,16 @@ class TrajectoryControllerTest : public ::testing::Test
ActivateTrajectoryController(separate_cmd_and_state_values);
}

<<<<<<< HEAD
void ActivateTrajectoryController(bool separate_cmd_and_state_values = false)
=======
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,
const std::vector<double> initial_acc_joints = INITIAL_ACC_JOINTS,
const std::vector<double> initial_eff_joints = INITIAL_EFF_JOINTS)
>>>>>>> fcc0847 ([JTC] Activate checks for parameter validation (#857))
{
std::vector<hardware_interface::LoanedCommandInterface> cmd_interfaces;
std::vector<hardware_interface::LoanedStateInterface> state_interfaces;
Expand Down Expand Up @@ -274,7 +307,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 958f492

Please sign in to comment.