From 2abc954b02bb4ada94bc7923d2adb9aa4a9f8c75 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 15 Jan 2025 21:04:10 +0000 Subject: [PATCH] Adding some tests for setting the speed scaling factor --- .../src/joint_trajectory_controller.cpp | 2 +- .../test/test_trajectory_controller.cpp | 59 +++++++++++++++++++ 2 files changed, 60 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c601aa06bc..d59756f3bf 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1693,7 +1693,7 @@ bool JointTrajectoryController::set_scaling_factor(const double scaling_factor) return false; } - if (scaling_factor_ != scaling_factor_) + if (scaling_factor != scaling_factor_) { RCLCPP_INFO( get_node()->get_logger().get_child("speed_scaling"), "New scaling factor will be %f", diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 610bc62af5..797f1b0e62 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -2209,3 +2209,62 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); } + +TEST_F(TrajectoryControllerTest, setting_scaling_factor_works_correctly) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params); + auto speed_scaling_pub = node_->create_publisher( + controller_name_ + "/speed_scaling_input", rclcpp::SystemDefaultsQoS()); + subscribeToState(executor); + + control_msgs::msg::SpeedScalingFactor msg; + msg.factor = 0.765; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + + updateController(); + + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); + EXPECT_EQ(state->speed_scaling_factor, 0.765); + + // 0.0 should work as an edge case + msg.factor = 0.0; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + updateController(); + executor.spin_some(); + state = getState(); + EXPECT_EQ(state->speed_scaling_factor, 0.0); + + // Sending a negative value will be ignored + msg.factor = 0.45; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + msg.factor = -0.12; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + updateController(); + executor.spin_some(); + state = getState(); + EXPECT_EQ(state->speed_scaling_factor, 0.45); +} + +TEST_F(TrajectoryControllerTest, scaling_factor_from_param) +{ + double initial_factor = 0.123; + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = { + rclcpp::Parameter("scaling_factor_initial_default", initial_factor), + }; + SetUpAndActivateTrajectoryController(executor, params); + subscribeToState(executor); + updateController(); + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); + EXPECT_EQ(state->speed_scaling_factor, initial_factor); +}