Skip to content

Commit

Permalink
[JTC] Make most parameters read-only (backport ros-controls#771)
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed Nov 27, 2023
1 parent 2d6c34d commit c26a91c
Show file tree
Hide file tree
Showing 7 changed files with 49 additions and 66 deletions.
3 changes: 1 addition & 2 deletions joint_trajectory_controller/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,7 @@ if(BUILD_TESTING)
target_link_libraries(test_trajectory joint_trajectory_controller)

ament_add_gmock(test_trajectory_controller
test/test_trajectory_controller.cpp
ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml)
test/test_trajectory_controller.cpp)
set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220)
target_link_libraries(test_trajectory_controller
joint_trajectory_controller
Expand Down
2 changes: 1 addition & 1 deletion joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1013,7 +1013,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
last_commanded_state_ = state;
}

// Should the controller start by holding position after on_configure?
// Should the controller start by holding position at the beginning of active state?
if (params_.start_with_holding)
{
add_new_trajectory_msg(set_hold_position());
Expand Down
7 changes: 7 additions & 0 deletions joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@ joint_trajectory_controller:
type: string_array,
default_value: [],
description: "Names of joints used by the controller",
read_only: true,
validation: {
unique<>: null,
}
Expand All @@ -11,6 +12,7 @@ joint_trajectory_controller:
type: string_array,
default_value: [],
description: "Names of the commanding joints used by the controller",
read_only: true,
validation: {
unique<>: null,
}
Expand All @@ -19,6 +21,7 @@ joint_trajectory_controller:
type: string_array,
default_value: [],
description: "Names of command interfaces to claim",
read_only: true,
validation: {
unique<>: null,
subset_of<>: [["position", "velocity", "acceleration", "effort",]],
Expand All @@ -29,6 +32,7 @@ joint_trajectory_controller:
type: string_array,
default_value: [],
description: "Names of state interfaces to claim",
read_only: true,
validation: {
unique<>: null,
subset_of<>: [["position", "velocity", "acceleration",]],
Expand All @@ -44,6 +48,7 @@ joint_trajectory_controller:
type: bool,
default_value: false,
description: "Run the controller in open-loop, i.e., read hardware states only when starting controller. This is useful when robot is not exactly following the commanded trajectory.",
read_only: true,
}
allow_integration_in_goal_trajectories: {
type: bool,
Expand All @@ -62,6 +67,7 @@ joint_trajectory_controller:
type: double,
default_value: 20.0,
description: "Rate status changes will be monitored",
read_only: true,
validation: {
gt_eq: [0.1]
}
Expand All @@ -70,6 +76,7 @@ joint_trajectory_controller:
type: string,
default_value: "splines",
description: "The type of interpolation to use, if any",
read_only: true,
validation: {
one_of<>: [["splines", "none"]],
}
Expand Down
12 changes: 0 additions & 12 deletions joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml
View file

This file was deleted.

3 changes: 1 addition & 2 deletions joint_trajectory_controller/test/test_trajectory_actions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest
{
setup_executor_ = true;

SetUpAndActivateTrajectoryController(
executor_, true, parameters, separate_cmd_and_state_values);
SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values);

SetUpActionClient();

Expand Down
53 changes: 25 additions & 28 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -138,11 +138,9 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names)
TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints)
{
rclcpp::executors::MultiThreadedExecutor executor;
SetUpTrajectoryController(executor);

// set command_joints parameter
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_);
traj_controller_->get_node()->set_parameter({command_joint_names_param});
SetUpTrajectoryController(executor, {command_joint_names_param});

const auto state = traj_controller_->get_node()->configure();
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
Expand Down Expand Up @@ -206,7 +204,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup)
rclcpp::executors::MultiThreadedExecutor executor;
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
SetUpAndActivateTrajectoryController(executor, true, params);
SetUpAndActivateTrajectoryController(executor, params);

// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
Expand Down Expand Up @@ -252,12 +250,11 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure)
TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters)
{
rclcpp::executors::MultiThreadedExecutor executor;
SetUpTrajectoryController(executor, false);
SetUpTrajectoryController(executor);
traj_controller_->get_node()->set_parameter(
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));

// This call is replacing the way parameters are set via launch
SetParameters();
traj_controller_->configure();
auto state = traj_controller_->get_state();
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
Expand Down Expand Up @@ -324,7 +321,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
TEST_P(TrajectoryControllerTestParameterized, state_topic_legacy_consistency)
{
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {});
SetUpAndActivateTrajectoryController(executor, {});
subscribeToStateLegacy();
updateController();

Expand Down Expand Up @@ -376,7 +373,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_legacy_consistency)
TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
{
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {});
SetUpAndActivateTrajectoryController(executor, {});
subscribeToState();
updateController();

Expand Down Expand Up @@ -478,7 +475,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup)
rclcpp::executors::MultiThreadedExecutor executor;

rclcpp::Parameter start_with_holding_parameter("start_with_holding", false);
SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter});
SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter});

constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
updateController(rclcpp::Duration(FIRST_POINT_TIME));
Expand All @@ -496,7 +493,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup)
rclcpp::executors::MultiThreadedExecutor executor;

rclcpp::Parameter start_with_holding_parameter("start_with_holding", true);
SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter});
SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter});

constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
updateController(rclcpp::Duration(FIRST_POINT_TIME));
Expand All @@ -520,7 +517,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized)
constexpr double k_p = 10.0;
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, false);
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false);
subscribeToState();

size_t n_joints = joint_names_.size();
Expand Down Expand Up @@ -629,7 +626,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_normalized)
constexpr double k_p = 10.0;
std::vector<rclcpp::Parameter> params = {
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
SetUpAndActivateTrajectoryController(executor, true, params, true, k_p, 0.0, true);
SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, true);
subscribeToState();

size_t n_joints = joint_names_.size();
Expand Down Expand Up @@ -750,7 +747,7 @@ void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_cou
rclcpp::Parameter state_publish_rate_param(
"state_publish_rate", static_cast<double>(target_msg_count));
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {state_publish_rate_param});
SetUpAndActivateTrajectoryController(executor, {state_publish_rate_param});

auto future_handle = std::async(std::launch::async, [&executor]() -> void { executor.spin(); });

Expand Down Expand Up @@ -819,7 +816,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid)
TEST_P(TrajectoryControllerTestParameterized, velocity_error)
{
rclcpp::executors::MultiThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {}, true);
SetUpAndActivateTrajectoryController(executor, {}, true);
subscribeToState();

size_t n_joints = joint_names_.size();
Expand Down Expand Up @@ -945,7 +942,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list)
rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true);

rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters});
SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters});

const double initial_joint1_cmd = joint_pos_[0];
const double initial_joint2_cmd = joint_pos_[1];
Expand Down Expand Up @@ -1017,7 +1014,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list_not_allowe
rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", false);

rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters});
SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters});

const double initial_joint1_cmd = joint_pos_[0];
const double initial_joint2_cmd = joint_pos_[1];
Expand Down Expand Up @@ -1099,7 +1096,7 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message)
rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false);
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(
executor, true, {partial_joints_parameters, allow_integration_parameters});
executor, {partial_joints_parameters, allow_integration_parameters});

trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;

Expand Down Expand Up @@ -1160,7 +1157,7 @@ TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted
{
rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true);
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {allow_integration_parameters});
SetUpAndActivateTrajectoryController(executor, {allow_integration_parameters});

trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;

Expand Down Expand Up @@ -1222,7 +1219,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace)
{
rclcpp::executors::SingleThreadedExecutor executor;
rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true);
SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters});
SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters});

subscribeToState();

Expand Down Expand Up @@ -1265,7 +1262,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace)
TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
{
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {});
SetUpAndActivateTrajectoryController(executor, {});
subscribeToState();

// TODO(anyone): add expectations for velocities and accelerations
Expand Down Expand Up @@ -1295,7 +1292,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory)
{
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {});
SetUpAndActivateTrajectoryController(executor, {});
subscribeToState();

std::vector<std::vector<double>> points_old{{{2., 3., 4.}, {4., 5., 6.}}};
Expand Down Expand Up @@ -1328,7 +1325,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur
{
rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true);
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters});
SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters});
subscribeToState();

RCLCPP_WARN(
Expand Down Expand Up @@ -1388,7 +1385,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
rclcpp::executors::SingleThreadedExecutor executor;
// default if false so it will not be actually set parameter
rclcpp::Parameter is_open_loop_parameters("open_loop_control", false);
SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true);
SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);

// goal setup
std::vector<double> first_goal = {3.3, 4.4, 5.5};
Expand Down Expand Up @@ -1489,7 +1486,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
{
rclcpp::executors::SingleThreadedExecutor executor;
rclcpp::Parameter is_open_loop_parameters("open_loop_control", true);
SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true);
SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);

// goal setup
std::vector<double> first_goal = {3.3, 4.4, 5.5};
Expand Down Expand Up @@ -1587,7 +1584,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co
joint_vel_[i] = std::numeric_limits<double>::quiet_NaN();
joint_acc_[i] = std::numeric_limits<double>::quiet_NaN();
}
SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true);
SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);

auto current_state_when_offset = traj_controller_->get_current_state_when_offset();

Expand Down Expand Up @@ -1634,7 +1631,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co
joint_vel_[i] = 0.25 + i;
joint_acc_[i] = 0.02 + i / 10.0;
}
SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true);
SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);

auto current_state_when_offset = traj_controller_->get_current_state_when_offset();

Expand Down Expand Up @@ -1676,7 +1673,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail)
rclcpp::Parameter("constraints.joint3.trajectory", state_tol)};

rclcpp::executors::MultiThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, false, {params}, true);
SetUpAndActivateTrajectoryController(executor, params, true);

// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100);
Expand Down Expand Up @@ -1708,7 +1705,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail)
rclcpp::Parameter("constraints.goal_time", goal_time)};

rclcpp::executors::MultiThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, false, {params}, true);
SetUpAndActivateTrajectoryController(executor, params, true);

// send msg
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100);
Expand Down
35 changes: 14 additions & 21 deletions joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -162,32 +162,26 @@ class TrajectoryControllerTest : public ::testing::Test
controller_name_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS());
}

void SetUpTrajectoryController(rclcpp::Executor & executor, bool use_local_parameters = true)
void SetUpTrajectoryController(
rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {})
{
traj_controller_ = std::make_shared<TestableJointTrajectoryController>();

if (use_local_parameters)
{
traj_controller_->set_joint_names(joint_names_);
traj_controller_->set_command_interfaces(command_interface_types_);
traj_controller_->set_state_interfaces(state_interface_types_);
}
auto ret = traj_controller_->init(controller_name_);
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);

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

void SetParameters()
{
auto node = traj_controller_->get_node();
const rclcpp::Parameter joint_names_param("joints", joint_names_);
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});
}

void SetPidParameters(
Expand All @@ -210,12 +204,11 @@ class TrajectoryControllerTest : public ::testing::Test
}

void SetUpAndActivateTrajectoryController(
rclcpp::Executor & executor, bool use_local_parameters = true,
const std::vector<rclcpp::Parameter> & parameters = {},
rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {},
bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
bool normalize_error = false)
{
SetUpTrajectoryController(executor, use_local_parameters);
SetUpTrajectoryController(executor);

// set pid parameters before configure
SetPidParameters(k_p, ff, normalize_error);
Expand Down

0 comments on commit c26a91c

Please sign in to comment.