diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7d66a2c3c1..59014e15db 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -965,11 +965,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( read_state_from_state_interfaces(last_commanded_state_); } - // 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()); - } + // The controller should start by holding position at the beginning of active state + add_new_trajectory_msg(set_hold_position()); rt_is_holding_.writeFromNonRT(true); // parse timeout parameter diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 083b362ab7..2570e11be9 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -82,11 +82,6 @@ joint_trajectory_controller: one_of<>: [["splines", "none"]], } } - start_with_holding: { - type: bool, - default_value: true, - description: "If true, start with holding position after activation. Otherwise, no command will be sent until the first trajectory is received.", - } allow_nonzero_velocity_at_trajectory_end: { type: bool, default_value: false, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 6d0057c8f8..9c4d20538c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -439,24 +439,6 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_tolerances) executor.cancel(); } -/** - * @brief check if hold on startup is deactivated - */ -TEST_P(TrajectoryControllerTestParameterized, no_hold_on_startup) -{ - rclcpp::executors::MultiThreadedExecutor executor; - - rclcpp::Parameter start_with_holding_parameter("start_with_holding", false); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); - - constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); - updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup without start_with_holding being set, we expect no active trajectory - ASSERT_FALSE(traj_controller_->has_active_traj()); - - executor.cancel(); -} - /** * @brief check if hold on startup */ @@ -464,12 +446,11 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) { rclcpp::executors::MultiThreadedExecutor executor; - rclcpp::Parameter start_with_holding_parameter("start_with_holding", true); - SetUpAndActivateTrajectoryController(executor, {start_with_holding_parameter}); + SetUpAndActivateTrajectoryController(executor, {}); constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - // after startup with start_with_holding being set, we expect an active trajectory: + // after startup, we expect an active trajectory: ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3};