Skip to content

Commit dc859d4

Browse files
christophfroehlichbmagyar
christophfroehlich
authored and
bmagyar
committed
[JTC] Make most parameters read-only (backport #771)
1 parent 8c715ac commit dc859d4

7 files changed

+49
-66
lines changed

‎joint_trajectory_controller/CMakeLists.txt

+1-2
Original file line numberDiff line numberDiff line change
@@ -58,8 +58,7 @@ if(BUILD_TESTING)
5858
target_link_libraries(test_trajectory joint_trajectory_controller)
5959

6060
ament_add_gmock(test_trajectory_controller
61-
test/test_trajectory_controller.cpp
62-
ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml)
61+
test/test_trajectory_controller.cpp)
6362
set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220)
6463
target_link_libraries(test_trajectory_controller
6564
joint_trajectory_controller

‎joint_trajectory_controller/src/joint_trajectory_controller.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -1013,7 +1013,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
10131013
last_commanded_state_ = state;
10141014
}
10151015

1016-
// Should the controller start by holding position after on_configure?
1016+
// Should the controller start by holding position at the beginning of active state?
10171017
if (params_.start_with_holding)
10181018
{
10191019
add_new_trajectory_msg(set_hold_position());

‎joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

+7
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@ joint_trajectory_controller:
33
type: string_array,
44
default_value: [],
55
description: "Names of joints used by the controller",
6+
read_only: true,
67
validation: {
78
unique<>: null,
89
}
@@ -11,6 +12,7 @@ joint_trajectory_controller:
1112
type: string_array,
1213
default_value: [],
1314
description: "Names of the commanding joints used by the controller",
15+
read_only: true,
1416
validation: {
1517
unique<>: null,
1618
}
@@ -19,6 +21,7 @@ joint_trajectory_controller:
1921
type: string_array,
2022
default_value: [],
2123
description: "Names of command interfaces to claim",
24+
read_only: true,
2225
validation: {
2326
unique<>: null,
2427
subset_of<>: [["position", "velocity", "acceleration", "effort",]],
@@ -29,6 +32,7 @@ joint_trajectory_controller:
2932
type: string_array,
3033
default_value: [],
3134
description: "Names of state interfaces to claim",
35+
read_only: true,
3236
validation: {
3337
unique<>: null,
3438
subset_of<>: [["position", "velocity", "acceleration",]],
@@ -44,6 +48,7 @@ joint_trajectory_controller:
4448
type: bool,
4549
default_value: false,
4650
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.",
51+
read_only: true,
4752
}
4853
allow_integration_in_goal_trajectories: {
4954
type: bool,
@@ -62,6 +67,7 @@ joint_trajectory_controller:
6267
type: double,
6368
default_value: 20.0,
6469
description: "Rate status changes will be monitored",
70+
read_only: true,
6571
validation: {
6672
gt_eq: [0.1]
6773
}
@@ -70,6 +76,7 @@ joint_trajectory_controller:
7076
type: string,
7177
default_value: "splines",
7278
description: "The type of interpolation to use, if any",
79+
read_only: true,
7380
validation: {
7481
one_of<>: [["splines", "none"]],
7582
}

‎joint_trajectory_controller/test/config/test_joint_trajectory_controller.yaml

-12
This file was deleted.

‎joint_trajectory_controller/test/test_trajectory_actions.cpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -72,8 +72,7 @@ class TestTrajectoryActions : public TrajectoryControllerTest
7272
{
7373
setup_executor_ = true;
7474

75-
SetUpAndActivateTrajectoryController(
76-
executor_, true, parameters, separate_cmd_and_state_values);
75+
SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values);
7776

7877
SetUpActionClient();
7978

‎joint_trajectory_controller/test/test_trajectory_controller.cpp

+25-28
Original file line numberDiff line numberDiff line change
@@ -138,11 +138,9 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names)
138138
TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints)
139139
{
140140
rclcpp::executors::MultiThreadedExecutor executor;
141-
SetUpTrajectoryController(executor);
142-
143141
// set command_joints parameter
144142
const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_);
145-
traj_controller_->get_node()->set_parameter({command_joint_names_param});
143+
SetUpTrajectoryController(executor, {command_joint_names_param});
146144

147145
const auto state = traj_controller_->get_node()->configure();
148146
ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
@@ -206,7 +204,7 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup)
206204
rclcpp::executors::MultiThreadedExecutor executor;
207205
std::vector<rclcpp::Parameter> params = {
208206
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)};
209-
SetUpAndActivateTrajectoryController(executor, true, params);
207+
SetUpAndActivateTrajectoryController(executor, params);
210208

211209
// send msg
212210
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250);
@@ -252,12 +250,11 @@ TEST_P(TrajectoryControllerTestParameterized, cleanup_after_configure)
252250
TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_parameters)
253251
{
254252
rclcpp::executors::MultiThreadedExecutor executor;
255-
SetUpTrajectoryController(executor, false);
253+
SetUpTrajectoryController(executor);
256254
traj_controller_->get_node()->set_parameter(
257255
rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true));
258256

259257
// This call is replacing the way parameters are set via launch
260-
SetParameters();
261258
traj_controller_->configure();
262259
auto state = traj_controller_->get_state();
263260
ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
@@ -324,7 +321,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param
324321
TEST_P(TrajectoryControllerTestParameterized, state_topic_legacy_consistency)
325322
{
326323
rclcpp::executors::SingleThreadedExecutor executor;
327-
SetUpAndActivateTrajectoryController(executor, true, {});
324+
SetUpAndActivateTrajectoryController(executor, {});
328325
subscribeToStateLegacy();
329326
updateController();
330327

@@ -376,7 +373,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_legacy_consistency)
376373
TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency)
377374
{
378375
rclcpp::executors::SingleThreadedExecutor executor;
379-
SetUpAndActivateTrajectoryController(executor, true, {});
376+
SetUpAndActivateTrajectoryController(executor, {});
380377
subscribeToState();
381378
updateController();
382379

@@ -478,7 +475,7 @@ TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup)
478475
rclcpp::executors::MultiThreadedExecutor executor;
479476

480477
rclcpp::Parameter start_with_holding_parameter("start_with_holding", false);
481-
SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter});
478+
SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter});
482479

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

498495
rclcpp::Parameter start_with_holding_parameter("start_with_holding", true);
499-
SetUpAndActivateTrajectoryController(executor, true, {start_with_holding_parameter});
496+
SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter});
500497

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

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

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

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

@@ -819,7 +816,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid)
819816
TEST_P(TrajectoryControllerTestParameterized, velocity_error)
820817
{
821818
rclcpp::executors::MultiThreadedExecutor executor;
822-
SetUpAndActivateTrajectoryController(executor, true, {}, true);
819+
SetUpAndActivateTrajectoryController(executor, {}, true);
823820
subscribeToState();
824821

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

947944
rclcpp::executors::SingleThreadedExecutor executor;
948-
SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters});
945+
SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters});
949946

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

10191016
rclcpp::executors::SingleThreadedExecutor executor;
1020-
SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters});
1017+
SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters});
10211018

10221019
const double initial_joint1_cmd = joint_pos_[0];
10231020
const double initial_joint2_cmd = joint_pos_[1];
@@ -1099,7 +1096,7 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message)
10991096
rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", false);
11001097
rclcpp::executors::SingleThreadedExecutor executor;
11011098
SetUpAndActivateTrajectoryController(
1102-
executor, true, {partial_joints_parameters, allow_integration_parameters});
1099+
executor, {partial_joints_parameters, allow_integration_parameters});
11031100

11041101
trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;
11051102

@@ -1160,7 +1157,7 @@ TEST_P(TrajectoryControllerTestParameterized, missing_positions_message_accepted
11601157
{
11611158
rclcpp::Parameter allow_integration_parameters("allow_integration_in_goal_trajectories", true);
11621159
rclcpp::executors::SingleThreadedExecutor executor;
1163-
SetUpAndActivateTrajectoryController(executor, true, {allow_integration_parameters});
1160+
SetUpAndActivateTrajectoryController(executor, {allow_integration_parameters});
11641161

11651162
trajectory_msgs::msg::JointTrajectory traj_msg, good_traj_msg;
11661163

@@ -1222,7 +1219,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace)
12221219
{
12231220
rclcpp::executors::SingleThreadedExecutor executor;
12241221
rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true);
1225-
SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters});
1222+
SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters});
12261223

12271224
subscribeToState();
12281225

@@ -1265,7 +1262,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace)
12651262
TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
12661263
{
12671264
rclcpp::executors::SingleThreadedExecutor executor;
1268-
SetUpAndActivateTrajectoryController(executor, true, {});
1265+
SetUpAndActivateTrajectoryController(executor, {});
12691266
subscribeToState();
12701267

12711268
// TODO(anyone): add expectations for velocities and accelerations
@@ -1295,7 +1292,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_ignore_old_trajectory)
12951292
TEST_P(TrajectoryControllerTestParameterized, test_ignore_partial_old_trajectory)
12961293
{
12971294
rclcpp::executors::SingleThreadedExecutor executor;
1298-
SetUpAndActivateTrajectoryController(executor, true, {});
1295+
SetUpAndActivateTrajectoryController(executor, {});
12991296
subscribeToState();
13001297

13011298
std::vector<std::vector<double>> points_old{{{2., 3., 4.}, {4., 5., 6.}}};
@@ -1328,7 +1325,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur
13281325
{
13291326
rclcpp::Parameter partial_joints_parameters("allow_partial_joints_goal", true);
13301327
rclcpp::executors::SingleThreadedExecutor executor;
1331-
SetUpAndActivateTrajectoryController(executor, true, {partial_joints_parameters});
1328+
SetUpAndActivateTrajectoryController(executor, {partial_joints_parameters});
13321329
subscribeToState();
13331330

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

13931390
// goal setup
13941391
std::vector<double> first_goal = {3.3, 4.4, 5.5};
@@ -1489,7 +1486,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
14891486
{
14901487
rclcpp::executors::SingleThreadedExecutor executor;
14911488
rclcpp::Parameter is_open_loop_parameters("open_loop_control", true);
1492-
SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true);
1489+
SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);
14931490

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

15921589
auto current_state_when_offset = traj_controller_->get_current_state_when_offset();
15931590

@@ -1634,7 +1631,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co
16341631
joint_vel_[i] = 0.25 + i;
16351632
joint_acc_[i] = 0.02 + i / 10.0;
16361633
}
1637-
SetUpAndActivateTrajectoryController(executor, true, {is_open_loop_parameters}, true);
1634+
SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);
16381635

16391636
auto current_state_when_offset = traj_controller_->get_current_state_when_offset();
16401637

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

16781675
rclcpp::executors::MultiThreadedExecutor executor;
1679-
SetUpAndActivateTrajectoryController(executor, false, {params}, true);
1676+
SetUpAndActivateTrajectoryController(executor, params, true);
16801677

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

17101707
rclcpp::executors::MultiThreadedExecutor executor;
1711-
SetUpAndActivateTrajectoryController(executor, false, {params}, true);
1708+
SetUpAndActivateTrajectoryController(executor, params, true);
17121709

17131710
// send msg
17141711
constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100);

‎joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

+14-21
Original file line numberDiff line numberDiff line change
@@ -162,32 +162,26 @@ class TrajectoryControllerTest : public ::testing::Test
162162
controller_name_ + "/joint_trajectory", rclcpp::SystemDefaultsQoS());
163163
}
164164

165-
void SetUpTrajectoryController(rclcpp::Executor & executor, bool use_local_parameters = true)
165+
void SetUpTrajectoryController(
166+
rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {})
166167
{
167168
traj_controller_ = std::make_shared<TestableJointTrajectoryController>();
168169

169-
if (use_local_parameters)
170-
{
171-
traj_controller_->set_joint_names(joint_names_);
172-
traj_controller_->set_command_interfaces(command_interface_types_);
173-
traj_controller_->set_state_interfaces(state_interface_types_);
174-
}
175-
auto ret = traj_controller_->init(controller_name_);
170+
auto node_options = rclcpp::NodeOptions();
171+
std::vector<rclcpp::Parameter> parameter_overrides;
172+
parameter_overrides.push_back(rclcpp::Parameter("joints", joint_names_));
173+
parameter_overrides.push_back(
174+
rclcpp::Parameter("command_interfaces", command_interface_types_));
175+
parameter_overrides.push_back(rclcpp::Parameter("state_interfaces", state_interface_types_));
176+
parameter_overrides.insert(parameter_overrides.end(), parameters.begin(), parameters.end());
177+
node_options.parameter_overrides(parameter_overrides);
178+
179+
auto ret = traj_controller_->init(controller_name_, "", node_options);
176180
if (ret != controller_interface::return_type::OK)
177181
{
178182
FAIL();
179183
}
180184
executor.add_node(traj_controller_->get_node()->get_node_base_interface());
181-
SetParameters();
182-
}
183-
184-
void SetParameters()
185-
{
186-
auto node = traj_controller_->get_node();
187-
const rclcpp::Parameter joint_names_param("joints", joint_names_);
188-
const rclcpp::Parameter cmd_interfaces_params("command_interfaces", command_interface_types_);
189-
const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_);
190-
node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params});
191185
}
192186

193187
void SetPidParameters(
@@ -210,12 +204,11 @@ class TrajectoryControllerTest : public ::testing::Test
210204
}
211205

212206
void SetUpAndActivateTrajectoryController(
213-
rclcpp::Executor & executor, bool use_local_parameters = true,
214-
const std::vector<rclcpp::Parameter> & parameters = {},
207+
rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {},
215208
bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
216209
bool normalize_error = false)
217210
{
218-
SetUpTrajectoryController(executor, use_local_parameters);
211+
SetUpTrajectoryController(executor);
219212

220213
// set pid parameters before configure
221214
SetPidParameters(k_p, ff, normalize_error);

0 commit comments

Comments
 (0)