Skip to content

Commit

Permalink
[JTC] Make most parameters read-only (#771)
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich authored Sep 11, 2023
1 parent fb46a77 commit 4e917db
Show file tree
Hide file tree
Showing 7 changed files with 47 additions and 64 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 @@ -60,8 +60,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 @@ -951,7 +951,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 @@ -54,6 +59,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 @@ -62,6 +68,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
49 changes: 23 additions & 26 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_consistency)
{
rclcpp::executors::SingleThreadedExecutor executor;
SetUpAndActivateTrajectoryController(executor, true, {});
SetUpAndActivateTrajectoryController(executor, {});
subscribeToState();
updateController();

Expand Down Expand Up @@ -426,7 +423,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 @@ -444,7 +441,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 @@ -468,7 +465,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 @@ -577,7 +574,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 @@ -720,7 +717,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 @@ -846,7 +843,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 @@ -918,7 +915,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 @@ -1000,7 +997,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 @@ -1061,7 +1058,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 @@ -1123,7 +1120,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 @@ -1166,7 +1163,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 @@ -1196,7 +1193,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 @@ -1229,7 +1226,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 @@ -1289,7 +1286,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 @@ -1390,7 +1387,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 @@ -1488,7 +1485,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 @@ -1535,7 +1532,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co
joint_vel_[i] = 0.25 + static_cast<double>(i);
joint_acc_[i] = 0.02 + static_cast<double>(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 @@ -1577,7 +1574,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 @@ -1609,7 +1606,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 4e917db

Please sign in to comment.