[JTC] Make most parameters read-only

joint_trajectory_controller/CMakeLists.txt

@@ -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

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -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());

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -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,
     }
@@ -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,
     }
@@ -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",]],
@@ -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",]],
@@ -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,
@@ -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]
     }
@@ -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"]],
     }

joint_trajectory_controller/test/test_trajectory_actions.cpp

@@ -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();
 

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -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);
@@ -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);
@@ -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());
@@ -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();
 
@@ -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));
@@ -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));
@@ -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();
@@ -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();
@@ -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();
@@ -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];
@@ -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];
@@ -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;
 
@@ -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;
 
@@ -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();
 
@@ -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
@@ -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.}}};
@@ -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(
@@ -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};
@@ -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};
@@ -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();
 
@@ -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();
 
@@ -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);
@@ -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);

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -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(
@@ -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);