Fix parameter updates and tests after rebase

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -127,10 +127,8 @@ controller_interface::return_type JointTrajectoryController::update(
   {
     params_ = param_listener_->get_params();
     default_tolerances_ = get_segment_tolerances(params_);
-
     // update gains of controller
-    // variable use_closed_loop_control_law_ is updated in on_configure only
-    if (use_external_control_law_)
+    if (traj_contr_)
     {
       if (traj_contr_->updateGainsRT() == false)
       {

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -362,30 +362,38 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
 
+  // with kp = 0.0
   SetUpAndActivateTrajectoryController(executor);
 
   updateControllerAsync();
-  auto pids = traj_controller_->get_pids();
+  auto pids = traj_controller_->get_traj_contr();
 
   if (traj_controller_->use_external_control_law())
   {
-    EXPECT_EQ(pids.size(), 3);
-    auto gain_0 = pids.at(0)->getGains();
-    EXPECT_EQ(gain_0.p_gain_, 0.0);
+    std::vector<double> tmp_command{0.0, 0.0, 0.0};
+    trajectory_msgs::msg::JointTrajectoryPoint error;
+    error.positions = {1.0, 0.0, 0.0};
+    error.velocities = {0.0, 0.0, 0.0};
+    trajectory_msgs::msg::JointTrajectoryPoint current;
+    trajectory_msgs::msg::JointTrajectoryPoint desired;
+    desired.velocities = {0.0, 0.0, 0.0};
+    rclcpp::Duration duration_since_start(std::chrono::milliseconds(250));
+    rclcpp::Duration period(std::chrono::milliseconds(100));
+
+    pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period);
+    EXPECT_EQ(tmp_command.at(0), 0.0);
 
     double kp = 1.0;
     SetPidParameters(kp);
     updateControllerAsync();
 
-    pids = traj_controller_->get_pids();
-    EXPECT_EQ(pids.size(), 3);
-    gain_0 = pids.at(0)->getGains();
-    EXPECT_EQ(gain_0.p_gain_, kp);
+    pids->computeCommands(tmp_command, current, error, desired, duration_since_start, period);
+    EXPECT_EQ(tmp_command.at(0), 1.0);
   }
   else
   {
     // nothing to check here, skip further test
-    EXPECT_EQ(pids.size(), 0);
+    EXPECT_EQ(pids, nullptr);
   }
 
   executor.cancel();

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -116,7 +116,11 @@ class TestableJointTrajectoryController
 
   bool is_open_loop() const { return params_.open_loop_control; }
 
-  std::vector<PidPtr> get_pids() const { return pids_; }
+  std::shared_ptr<joint_trajectory_controller_plugins::TrajectoryControllerBase> get_traj_contr()
+    const
+  {
+    return traj_contr_;
+  }
 
   joint_trajectory_controller::SegmentTolerances get_tolerances() const
   {
@@ -208,7 +212,8 @@ class TrajectoryControllerTest : public ::testing::Test
     for (size_t i = 0; i < joint_names_.size(); ++i)
     {
       const std::string prefix = "gains." + joint_names_[i];
-      const rclcpp::Parameter angle_wraparound(prefix + ".angle_wraparound", angle_wraparound_default);
+      const rclcpp::Parameter angle_wraparound(
+        prefix + ".angle_wraparound", angle_wraparound_default);
       node->set_parameters({angle_wraparound});
     }
   }