diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 11cb30b9ab..077afa6f7c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -2675,8 +2675,10 @@ TEST_F(TrajectoryControllerTest, setting_scaling_factor_works_correctly) rclcpp::executors::MultiThreadedExecutor executor; std::vector params = {}; SetUpAndActivateTrajectoryController(executor, params); + // Create a QoS profile that matches the controller's speed_scaling subscriber + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(); auto speed_scaling_pub = node_->create_publisher( - controller_name_ + "/speed_scaling_input", rclcpp::SystemDefaultsQoS()); + controller_name_ + "/speed_scaling_input", qos); subscribeToState(executor); control_msgs::msg::SpeedScalingFactor msg; @@ -2775,8 +2777,10 @@ TEST_F(TrajectoryControllerTest, scaling_state_interface_sets_value) }; SetUpAndActivateTrajectoryController(executor, params); + // Create a QoS profile that matches the controller's speed_scaling subscriber + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(); auto speed_scaling_pub = node_->create_publisher( - controller_name_ + "/speed_scaling_input", rclcpp::SystemDefaultsQoS()); + controller_name_ + "/speed_scaling_input", qos); subscribeToState(executor); updateController(); // Spin to receive latest state @@ -2810,8 +2814,10 @@ TEST_F(TrajectoryControllerTest, scaling_command_interface_sets_value) }; SetUpAndActivateTrajectoryController(executor, params); + // Create a QoS profile that matches the controller's speed_scaling subscriber + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local(); auto speed_scaling_pub = node_->create_publisher( - controller_name_ + "/speed_scaling_input", rclcpp::SystemDefaultsQoS()); + controller_name_ + "/speed_scaling_input", qos); subscribeToState(executor); updateController(); // Spin to receive latest state