Fix parameter updates and tests after rebase

christophfroehlich · Nov 19, 2023 · ae82875 · ae82875
1 parent 69ad1b6
commit ae82875
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 31 deletions.
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -126,26 +126,18 @@ controller_interface::return_type JointTrajectoryController::update(
   if (param_listener_->is_old(params_))
   {
     params_ = param_listener_->get_params();
-    // use_closed_loop_pid_adapter_ is updated in on_configure only
-    if (use_closed_loop_pid_adapter_)
+    default_tolerances_ = get_segment_tolerances(params_);
+    // update gains of controller
+    if (traj_contr_)
     {
-      update_pids();
-      default_tolerances_ = get_segment_tolerances(params_);
+      if (traj_contr_->updateGainsRT() == false)
+      {
+        RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller");
+        return controller_interface::return_type::ERROR;
+      }
     }
   }
 
-  // update gains of controller
-  // TODO(christophfroehlich) activate this
-  // once https://github.com/ros-controls/ros2_controllers/pull/761 is merged
-  // if (traj_contr_)
-  // {
-  //   if(traj_contr_->updateGainsRT() == false)
-  //   {
-  //     RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller");
-  //     return controller_interface::return_type::ERROR;
-  //   }
-  // }
-
   auto compute_error_for_joint = [&](
                                    JointTrajectoryPoint & error, size_t index,
                                    const JointTrajectoryPoint & current,
@@ -1071,7 +1063,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   {
     cmd_timeout_ = 0.0;
   }
-
   return CallbackReturn::SUCCESS;
 }
 

diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -420,30 +420,38 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
 
+  // with kp = 0.0
   SetUpAndActivateTrajectoryController(executor);
 
   updateController();
-  auto pids = traj_controller_->get_pids();
+  auto pids = traj_controller_->get_traj_contr();
 
-  if (traj_controller_->use_closed_loop_pid_adapter())
+  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);
     updateController();
 
-    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();

diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/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_;
+  }
 
   bool has_active_traj() const { return has_active_trajectory(); }
 
@@ -193,7 +197,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});
     }
   }
@@ -230,12 +235,13 @@ class TrajectoryControllerTest : public ::testing::Test
   {
     SetUpTrajectoryController(executor);
 
+    SetJointParameters(angle_wraparound);
+
     // 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);
 
-    SetJointParameters(normalize_error);
-
+    // set optional parameters
     for (const auto & param : parameters)
     {
       traj_controller_->get_node()->set_parameter(param);