From f45edf72f810e1ec1a965051f860666bc3612551 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Wed, 15 Nov 2023 21:21:08 +0000
Subject: [PATCH] Fix tests due to allow_nonzero..

---
 .../test/test_trajectory_controller_utils.hpp | 25 +++++++++++--------
 1 file changed, 14 insertions(+), 11 deletions(-)

diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 4252b5443c..062ed09baa 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -228,21 +228,24 @@ class TrajectoryControllerTest : public ::testing::Test
     const std::vector<double> initial_acc_joints = INITIAL_ACC_JOINTS,
     const std::vector<double> initial_eff_joints = INITIAL_EFF_JOINTS)
   {
-    SetUpTrajectoryController(executor);
-
-    // add this to simplify tests, can be overwritten by parameters
-    rclcpp::Parameter nonzero_vel_parameter("allow_nonzero_velocity_at_trajectory_end", true);
-    traj_controller_->get_node()->set_parameter(nonzero_vel_parameter);
+    auto has_nonzero_vel_param =
+      std::find_if(
+        parameters.begin(), parameters.end(),
+        [](const rclcpp::Parameter & param) {
+          return param.get_name() == "allow_nonzero_velocity_at_trajectory_end";
+        }) != parameters.end();
+
+    std::vector<rclcpp::Parameter> parameters_local = parameters;
+    if (!has_nonzero_vel_param)
+    {
+      // add this to simplify tests, if not set already
+      parameters_local.emplace_back("allow_nonzero_velocity_at_trajectory_end", true);
+    }
+    SetUpTrajectoryController(executor, parameters_local);
 
     // set pid parameters before configure
     SetPidParameters(k_p, ff, angle_wraparound);
 
-    // set optional parameters
-    for (const auto & param : parameters)
-    {
-      traj_controller_->get_node()->set_parameter(param);
-    }
-
     traj_controller_->get_node()->configure();
 
     ActivateTrajectoryController(