diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt
index 58a6378e19..eca4220adf 100644
--- a/joint_trajectory_controller/CMakeLists.txt
+++ b/joint_trajectory_controller/CMakeLists.txt
@@ -20,6 +20,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   tl_expected
   trajectory_msgs
   urdf
+  joint_trajectory_controller_plugins
 )
 
 find_package(ament_cmake REQUIRED)
@@ -113,4 +114,5 @@ install(TARGETS
 
 ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 ament_package()
diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst
index dbb50fcbeb..f7c9dbb94c 100644
--- a/joint_trajectory_controller/doc/parameters.rst
+++ b/joint_trajectory_controller/doc/parameters.rst
@@ -11,12 +11,24 @@ This controller uses the `generate_parameter_library <https://github.com/PickNik
 List of parameters
 =========================
 
+joint_trajectory_controller
+---------------------------
+
 .. generate_parameter_library_details::
   ../src/joint_trajectory_controller_parameters.yaml
   parameters_context.yaml
 
+pid_trajectory_plugin
+-----------------------------------
+
+.. generate_parameter_library_details::
+  ../../joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml
+  ../../joint_trajectory_controller_plugins/doc/parameters_context.yaml
+
 An example parameter file
 =========================
 
-.. generate_parameter_library_default::
-  ../src/joint_trajectory_controller_parameters.yaml
+An example parameter file for this controller can be found in `the test directory <https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/joint_trajectory_controller/test/test_joint_trajectory_controller_params.yaml>`_:
+
+.. literalinclude:: ../test/test_joint_trajectory_controller_params.yaml
+   :language: yaml
diff --git a/joint_trajectory_controller/doc/parameters_context.yaml b/joint_trajectory_controller/doc/parameters_context.yaml
index 9623899245..72871d0179 100644
--- a/joint_trajectory_controller/doc/parameters_context.yaml
+++ b/joint_trajectory_controller/doc/parameters_context.yaml
@@ -1,18 +1,2 @@
 constraints:
     Default values for tolerances if no explicit values are set in the ``JointTrajectory`` message.
-
-gains: |
-  If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint.
-  This structure contains the controller gains for every joint with the control law
-
-  .. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v)
-
-  with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below),
-  the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively.
-
-  If the joint is of continuous type, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`,
-  i.e., the shortest rotation to the target position is the desired motion.
-  Otherwise  :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured
-  position :math:`s` from the state interface.
-
-  If you want to turn off the PIDs (open loop control), set the feedback gains to zero.
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
index ad4abb6335..4d8288c840 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -23,12 +23,13 @@
 #include "control_msgs/action/follow_joint_trajectory.hpp"
 #include "control_msgs/msg/joint_trajectory_controller_state.hpp"
 #include "control_msgs/srv/query_trajectory_state.hpp"
-#include "control_toolbox/pid.hpp"
 #include "controller_interface/controller_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "joint_trajectory_controller/interpolation_methods.hpp"
 #include "joint_trajectory_controller/tolerances.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
+#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp"
+#include "pluginlib/class_loader.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/time.hpp"
@@ -105,7 +106,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
   // Storing command joint names for interfaces
   std::vector<std::string> command_joint_names_;
-
+#if RCLCPP_VERSION_MAJOR >= 17
+  // TODO(anyone) remove this if there is another way to lock command_joints parameter
+  rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr lock_cmd_joint_names;
+#endif
   // Parameters from ROS for joint_trajectory_controller
   std::shared_ptr<ParamListener> param_listener_;
   Params params_;
@@ -138,11 +142,13 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   bool has_effort_command_interface_ = false;
 
   /// If true, a velocity feedforward term plus corrective PID term is used
-  bool use_closed_loop_pid_adapter_ = false;
-  using PidPtr = std::shared_ptr<control_toolbox::Pid>;
-  std::vector<PidPtr> pids_;
-  // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
-  std::vector<double> ff_velocity_scale_;
+  bool use_external_control_law_ = false;
+  // class loader for actual trajectory controller
+  std::shared_ptr<
+    pluginlib::ClassLoader<joint_trajectory_controller_plugins::TrajectoryControllerBase>>
+    traj_controller_loader_;
+  // The actual trajectory controller
+  std::shared_ptr<joint_trajectory_controller_plugins::TrajectoryControllerBase> traj_contr_;
   // Configuration for every joint if it wraps around (ie. is continuous, position error is
   // normalized)
   std::vector<bool> joints_angle_wraparound_;
@@ -215,7 +221,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void sort_to_local_joint_order(
     std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg) const;
   bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
-  void add_new_trajectory_msg(
+  void add_new_trajectory_msg_nonRT(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
+  void add_new_trajectory_msg_RT(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
   bool validate_trajectory_point_field(
     size_t joint_names_size, const std::vector<double> & vector_field,
@@ -262,8 +270,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     std::shared_ptr<control_msgs::srv::QueryTrajectoryState::Response> response);
 
 private:
-  void update_pids();
-
   bool contains_interface_type(
     const std::vector<std::string> & interface_type_list, const std::string & interface_type);
 
diff --git a/joint_trajectory_controller/package.xml b/joint_trajectory_controller/package.xml
index 88a0ccafce..a879ddef73 100644
--- a/joint_trajectory_controller/package.xml
+++ b/joint_trajectory_controller/package.xml
@@ -34,6 +34,7 @@
   <depend>tl_expected</depend>
   <depend>trajectory_msgs</depend>
   <depend>urdf</depend>
+  <depend>joint_trajectory_controller_plugins</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
index 6890cea55e..b490dd0227 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp
+++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -25,7 +25,6 @@
 #include "angles/angles.h"
 #include "controller_interface/helpers.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
-#include "joint_trajectory_controller/trajectory.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "rclcpp/logging.hpp"
 #include "rclcpp/qos.hpp"
@@ -41,10 +40,19 @@
 #include "urdf/model.h"
 #endif
 
+#include "joint_trajectory_controller/trajectory.hpp"
+
 namespace joint_trajectory_controller
 {
 JointTrajectoryController::JointTrajectoryController()
-: controller_interface::ControllerInterface(), dof_(0), num_cmd_joints_(0)
+: controller_interface::ControllerInterface(),
+  dof_(0),
+  num_cmd_joints_(0),
+  traj_controller_loader_(
+    std::make_shared<
+      pluginlib::ClassLoader<joint_trajectory_controller_plugins::TrajectoryControllerBase>>(
+      "joint_trajectory_controller_plugins",
+      "joint_trajectory_controller_plugins::TrajectoryControllerBase"))
 {
 }
 
@@ -141,24 +149,30 @@ controller_interface::return_type JointTrajectoryController::update(
   {
     params_ = param_listener_->get_params();
     default_tolerances_ = get_segment_tolerances(logger, params_);
-    // update the PID gains
-    // variable use_closed_loop_pid_adapter_ is updated in on_configure only
-    if (use_closed_loop_pid_adapter_)
+    // update gains of controller
+    if (traj_contr_)
     {
-      update_pids();
+      if (traj_contr_->update_gains_rt() == false)
+      {
+        RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller");
+        return controller_interface::return_type::ERROR;
+      }
     }
   }
 
   // don't update goal after we sampled the trajectory to avoid any racecondition
   const auto active_goal = *rt_active_goal_.readFromRT();
 
-  // Check if a new trajectory message has been received from Non-RT threads
-  const auto current_trajectory_msg = current_trajectory_->get_trajectory_msg();
+  // Check if a new external message has been received from nonRT threads
+  auto current_external_msg = current_trajectory_->get_trajectory_msg();
   auto new_external_msg = new_trajectory_msg_.readFromRT();
-  // Discard, if a goal is pending but still not active (somewhere stuck in goal_handle_timer_)
+  // Discard,
+  //  if a goal is pending but still not active (somewhere stuck in goal_handle_timer_)
+  //  and if traj_contr_: wait until control law is computed by the traj_contr_
   if (
-    current_trajectory_msg != *new_external_msg &&
-    (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false)
+    current_external_msg != *new_external_msg &&
+    (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false &&
+    (traj_contr_ == nullptr || traj_contr_->is_ready()))
   {
     fill_partial_goal(*new_external_msg);
     sort_to_local_joint_order(*new_external_msg);
@@ -193,6 +207,11 @@ controller_interface::return_type JointTrajectoryController::update(
         current_trajectory_->set_point_before_trajectory_msg(
           time, state_current_, joints_angle_wraparound_);
       }
+      if (traj_contr_)
+      {
+        // switch RT buffer of traj_contr_
+        traj_contr_->start();
+      }
       traj_time_ = time;
     }
     else
@@ -234,8 +253,7 @@ controller_interface::return_type JointTrajectoryController::update(
       {
         RCLCPP_WARN(logger, "Aborted due to command timeout");
 
-        new_trajectory_msg_.reset();
-        new_trajectory_msg_.initRT(set_hold_position());
+        add_new_trajectory_msg_RT(set_hold_position());
       }
 
       // Check state/goal tolerance
@@ -279,21 +297,11 @@ controller_interface::return_type JointTrajectoryController::update(
       // set values for next hardware write() if tolerance is met
       if (!tolerance_violated_while_moving && within_goal_time)
       {
-        if (use_closed_loop_pid_adapter_)
+        if (traj_contr_)
         {
-          // Update PIDs
-          for (auto i = 0ul; i < num_cmd_joints_; ++i)
-          {
-            // If effort interface only, add desired effort as feed forward
-            // If velocity interface, ignore desired effort
-            size_t index_cmd_joint = map_cmd_to_joints_[i];
-            tmp_command_[index_cmd_joint] =
-              (command_next_.velocities[index_cmd_joint] * ff_velocity_scale_[i]) +
-              (has_effort_command_interface_ ? command_next_.effort[index_cmd_joint] : 0.0) +
-              pids_[i]->compute_command(
-                state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint],
-                period);
-          }
+          traj_contr_->compute_commands(
+            tmp_command_, state_current_, state_error_, command_next_,
+            time - current_trajectory_->time_from_start(), period);
         }
 
         // set values for next hardware write()
@@ -303,7 +311,7 @@ controller_interface::return_type JointTrajectoryController::update(
         }
         if (has_velocity_command_interface_)
         {
-          if (use_closed_loop_pid_adapter_)
+          if (use_external_control_law_)
           {
             assign_interface_from_point(joint_command_interface_[1], tmp_command_);
           }
@@ -318,8 +326,16 @@ controller_interface::return_type JointTrajectoryController::update(
         }
         if (has_effort_command_interface_)
         {
-          if (use_closed_loop_pid_adapter_)
+          if (use_external_control_law_)
           {
+            // add feedforward effort
+            for (auto i = 0ul; i < num_cmd_joints_; ++i)
+            {
+              // If effort interface only, add desired effort as feed forward
+              // If velocity interface, ignore desired effort
+              size_t index_cmd_joint = map_cmd_to_joints_[i];
+              tmp_command_[index_cmd_joint] += command_next_.effort[index_cmd_joint];
+            }
             assign_interface_from_point(joint_command_interface_[3], tmp_command_);
           }
           else
@@ -360,8 +376,7 @@ controller_interface::return_type JointTrajectoryController::update(
 
           RCLCPP_WARN(logger, "Aborted due to state tolerance violation");
 
-          new_trajectory_msg_.reset();
-          new_trajectory_msg_.initRT(set_hold_position());
+          add_new_trajectory_msg_RT(set_hold_position());
         }
         // check goal tolerance
         else if (!before_last_point)
@@ -379,8 +394,7 @@ controller_interface::return_type JointTrajectoryController::update(
 
             RCLCPP_INFO(logger, "Goal reached, success!");
 
-            new_trajectory_msg_.reset();
-            new_trajectory_msg_.initRT(set_success_trajectory_point());
+            add_new_trajectory_msg_RT(set_success_trajectory_point());
           }
           else if (!within_goal_time)
           {
@@ -398,8 +412,7 @@ controller_interface::return_type JointTrajectoryController::update(
 
             RCLCPP_WARN(logger, "%s", error_string.c_str());
 
-            new_trajectory_msg_.reset();
-            new_trajectory_msg_.initRT(set_hold_position());
+            add_new_trajectory_msg_RT(set_hold_position());
           }
         }
       }
@@ -408,16 +421,14 @@ controller_interface::return_type JointTrajectoryController::update(
         // we need to ensure that there is no pending goal -> we get a race condition otherwise
         RCLCPP_ERROR(logger, "Holding position due to state tolerance violation");
 
-        new_trajectory_msg_.reset();
-        new_trajectory_msg_.initRT(set_hold_position());
+        add_new_trajectory_msg_RT(set_hold_position());
       }
       else if (
         !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false)
       {
         RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position...");
 
-        new_trajectory_msg_.reset();
-        new_trajectory_msg_.initRT(set_hold_position());
+        add_new_trajectory_msg_RT(set_hold_position());
       }
       // else, run another cycle while waiting for outside_goal_tolerance
       // to be satisfied (will stay in this state until new message arrives)
@@ -725,6 +736,38 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     command_joint_names_ = params_.joints;
     RCLCPP_INFO(
       logger, "No specific joint names are used for command interfaces. Using 'joints' parameter.");
+
+    // set the parameter for the controller plugin
+    auto result =
+      get_node()->set_parameter(rclcpp::Parameter("command_joints", command_joint_names_));
+    if (result.successful == false)
+    {
+      RCLCPP_ERROR(logger, "Failed to set 'command_joints' parameter");
+      return CallbackReturn::FAILURE;
+    }
+#if RCLCPP_VERSION_MAJOR >= 17
+    // TODO(christophfroehlich) how to lock the parameter (set read_only to false)?
+    // Setting it to read_only but override is not supported
+    // https://github.com/ros2/rclcpp/issues/1762 get_node()->undeclare_parameter("command_joints");
+    // rcl_interfaces::msg::ParameterDescriptor parameter_descriptor;
+    // parameter_descriptor.read_only = true;
+    // get_node()->declare_parameter("command_joints",
+    //  rclcpp::ParameterValue(command_joint_names_), parameter_descriptor);
+    lock_cmd_joint_names = get_node()->add_pre_set_parameters_callback(
+      [this](std::vector<rclcpp::Parameter> & parameters)
+      {
+        for (auto & parameter : parameters)
+        {
+          if (parameter.get_name() == "command_joints")
+          {
+            RCLCPP_ERROR(
+              get_node()->get_logger(),
+              "The parameter 'command_joints' is read-only. You can't change it.");
+            parameter = rclcpp::Parameter("command_joints", command_joint_names_);
+          }
+        }
+      });
+#endif
   }
   num_cmd_joints_ = command_joint_names_.size();
 
@@ -781,20 +824,55 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT);
 
   // if there is only velocity or if there is effort command interface
-  // then use also PID adapter
-  use_closed_loop_pid_adapter_ =
+  // then use external control law
+  use_external_control_law_ =
     (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&
      !params_.open_loop_control) ||
     (has_effort_command_interface_ && params_.command_interfaces.size() == 1);
 
   tmp_command_.resize(dof_, 0.0);
 
-  if (use_closed_loop_pid_adapter_)
+  if (use_external_control_law_)
   {
-    pids_.resize(num_cmd_joints_);
-    ff_velocity_scale_.resize(num_cmd_joints_);
+    try
+    {
+      traj_contr_ =
+        traj_controller_loader_->createSharedInstance(params_.controller_plugin.c_str());
+    }
+    catch (pluginlib::PluginlibException & ex)
+    {
+      RCLCPP_FATAL(
+        logger, "The trajectory controller plugin `%s` failed to load for some reason. Error: %s\n",
+        params_.controller_plugin.c_str(), ex.what());
+      return CallbackReturn::FAILURE;
+    }
+    if (traj_contr_->initialize(get_node(), map_cmd_to_joints_) == false)
+    {
+      RCLCPP_FATAL(
+        logger,
+        "The trajectory controller plugin `%s` failed to initialize for some reason. Aborting.",
+        params_.controller_plugin.c_str());
+      return CallbackReturn::FAILURE;
+    }
+    else
+    {
+      if (traj_contr_->configure() == false)
+      {
+        RCLCPP_FATAL(
+          logger,
+          "The trajectory controller plugin `%s` failed to configure for some reason. Aborting.",
+          params_.controller_plugin.c_str());
+        return CallbackReturn::FAILURE;
+      }
+      else
+      {
+        RCLCPP_INFO(
+          logger, "The trajectory controller plugin `%s` was loaded and configured.",
+          params_.controller_plugin.c_str());
+      }
+    }
 
-    update_pids();
+    tmp_command_.resize(dof_, 0.0);
   }
 
   if (params_.state_interfaces.empty())
@@ -987,7 +1065,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
           command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
     {
       RCLCPP_ERROR(
-        logger, "Expected %zu '%s' command interfaces, got %zu.", num_cmd_joints_,
+        get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_cmd_joints_,
         interface.c_str(), joint_command_interface_[index].size());
       return CallbackReturn::ERROR;
     }
@@ -1035,8 +1113,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   }
   last_commanded_time_ = rclcpp::Time();
 
+  // activate traj_contr_, e.g., update gains
+  if (traj_contr_ && traj_contr_->activate() == false)
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "Error during trajectory controller activation.");
+    return CallbackReturn::ERROR;
+  }
+
   // The controller should start by holding position at the beginning of active state
-  add_new_trajectory_msg(set_hold_position());
+  add_new_trajectory_msg_nonRT(set_hold_position());
   rt_is_holding_.writeFromNonRT(true);
 
   // parse timeout parameter
@@ -1113,6 +1198,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate(
 
   current_trajectory_.reset();
 
+  // e.g., reset integral states
+  if (traj_contr_)
+  {
+    traj_contr_->reset();
+  }
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -1131,14 +1222,6 @@ bool JointTrajectoryController::reset()
   subscriber_is_active_ = false;
   joint_command_subscriber_.reset();
 
-  for (const auto & pid : pids_)
-  {
-    if (pid)
-    {
-      pid->reset();
-    }
-  }
-
   current_trajectory_.reset();
 
   return true;
@@ -1186,10 +1269,10 @@ void JointTrajectoryController::topic_callback(
   // always replace old msg with new one for now
   if (subscriber_is_active_)
   {
-    add_new_trajectory_msg(msg);
+    add_new_trajectory_msg_nonRT(msg);
     rt_is_holding_.writeFromNonRT(false);
   }
-};
+}
 
 rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback(
   const rclcpp_action::GoalUUID &, std::shared_ptr<const FollowJTrajAction::Goal> goal)
@@ -1232,7 +1315,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback
     rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
 
     // Enter hold current position mode
-    add_new_trajectory_msg(set_hold_position());
+    add_new_trajectory_msg_nonRT(set_hold_position());
   }
   return rclcpp_action::CancelResponse::ACCEPT;
 }
@@ -1249,7 +1332,7 @@ void JointTrajectoryController::goal_accepted_callback(
     auto traj_msg =
       std::make_shared<trajectory_msgs::msg::JointTrajectory>(goal_handle->get_goal()->trajectory);
 
-    add_new_trajectory_msg(traj_msg);
+    add_new_trajectory_msg_nonRT(traj_msg);
     rt_is_holding_.writeFromNonRT(false);
   }
 
@@ -1576,10 +1659,39 @@ bool JointTrajectoryController::validate_trajectory_msg(
   return true;
 }
 
-void JointTrajectoryController::add_new_trajectory_msg(
+void JointTrajectoryController::add_new_trajectory_msg_nonRT(
   const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)
 {
   new_trajectory_msg_.writeFromNonRT(traj_msg);
+
+  // compute control law
+  if (traj_contr_)
+  {
+    // this can take some time; trajectory won't start until control law is computed
+    if (traj_contr_->compute_control_law_non_rt(traj_msg) == false)
+    {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
+    }
+  }
+}
+
+void JointTrajectoryController::add_new_trajectory_msg_RT(
+  const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg)
+{
+  // TODO(christophfroehlich): Need a lock-free write here
+  // See https://github.com/ros-controls/ros2_controllers/issues/168
+  new_trajectory_msg_.reset();
+  new_trajectory_msg_.initRT(traj_msg);
+
+  // compute control law
+  if (traj_contr_)
+  {
+    // this is used for set_hold_position() only -> this should (and must) not take a long time
+    if (traj_contr_->compute_control_law_rt(traj_msg) == false)
+    {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
+    }
+  }
 }
 
 void JointTrajectoryController::preempt_active_goal()
@@ -1587,6 +1699,7 @@ void JointTrajectoryController::preempt_active_goal()
   const auto active_goal = *rt_active_goal_.readFromNonRT();
   if (active_goal)
   {
+    add_new_trajectory_msg_nonRT(set_hold_position());
     auto action_res = std::make_shared<FollowJTrajAction::Result>();
     action_res->set__error_code(FollowJTrajAction::Result::INVALID_GOAL);
     action_res->set__error_string("Current goal cancelled due to new incoming action.");
@@ -1669,26 +1782,6 @@ bool JointTrajectoryController::has_active_trajectory() const
   return current_trajectory_ != nullptr && current_trajectory_->has_trajectory_msg();
 }
 
-void JointTrajectoryController::update_pids()
-{
-  for (size_t i = 0; i < num_cmd_joints_; ++i)
-  {
-    const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i]));
-    if (pids_[i])
-    {
-      // update PIDs with gains from ROS parameters
-      pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
-    }
-    else
-    {
-      // Init PIDs with gains from ROS parameters
-      pids_[i] = std::make_shared<control_toolbox::Pid>(
-        gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
-    }
-    ff_velocity_scale_[i] = gains.ff_velocity_scale;
-  }
-}
-
 void JointTrajectoryController::init_hold_position_msg()
 {
   hold_position_msg_ptr_ = std::make_shared<trajectory_msgs::msg::JointTrajectory>();
diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
index 926366cde6..04982ae609 100644
--- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
+++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml
@@ -17,7 +17,7 @@ joint_trajectory_controller:
         * JTC is used in a controller chain where command and state interfaces don't have same names.
 
         * If the number of command joints is smaller than the degrees-of-freedom. For example to track the state and error of passive joints. ``command_joints`` must then be a subset of ``joints``.",
-    read_only: true,
+    read_only: false, # should be set to true after configuration of the controller
     validation: {
       unique<>: null,
     }
@@ -72,11 +72,12 @@ joint_trajectory_controller:
       Therefore it is important set command interfaces to NaN (i.e., ``std::numeric_limits<double>::quiet_NaN()``) or state values when the hardware is started.\n",
     read_only: true,
   }
-  allow_integration_in_goal_trajectories: {
-    type: bool,
-    default_value: false,
-    description: "Allow integration in goal trajectories to accept goals without position or velocity specified",
-  }
+  allow_integration_in_goal_trajectories:
+    {
+      type: bool,
+      default_value: false,
+      description: "Allow integration in goal trajectories to accept goals without position or velocity specified",
+    }
   set_last_command_interface_value_as_state_on_activation: {
     type: bool,
     default_value: true,
@@ -91,78 +92,61 @@ joint_trajectory_controller:
       gt_eq: [0.1]
     }
   }
-  interpolation_method: {
-    type: string,
-    default_value: "splines",
-    description: "The type of interpolation to use, if any",
-    read_only: true,
-    validation: {
-      one_of<>: [["splines", "none"]],
+  interpolation_method:
+    {
+      type: string,
+      default_value: "splines",
+      description: "The type of interpolation to use, if any",
+      read_only: true,
+      validation: { one_of<>: [["splines", "none"]] },
+    }
+  allow_nonzero_velocity_at_trajectory_end:
+    {
+      type: bool,
+      default_value: false,
+      description: "If false, the last velocity point has to be zero or the goal will be rejected",
     }
-  }
-  allow_nonzero_velocity_at_trajectory_end: {
-    type: bool,
-    default_value: false,
-    description: "If false, the last velocity point has to be zero or the goal will be rejected",
-  }
   cmd_timeout: {
-    type: double,
-    default_value: 0.0, # seconds
-    description: "Timeout after which the input command is considered stale.
-     Timeout is counted from the end of the trajectory (the last point).
-     ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored.
-     If zero, timeout is deactivated",
-  }
-  gains:
-    __map_joints:
-      p: {
-        type: double,
-        default_value: 0.0,
-        description: "Proportional gain :math:`k_p` for PID"
-      }
-      i: {
-        type: double,
-        default_value: 0.0,
-        description: "Integral gain :math:`k_i` for PID"
-      }
-      d: {
-        type: double,
-        default_value: 0.0,
-        description: "Derivative gain :math:`k_d` for PID"
-      }
-      i_clamp: {
-        type: double,
-        default_value: 0.0,
-        description: "Integral clamp. Symmetrical in both positive and negative direction."
-      }
-      ff_velocity_scale: {
-        type: double,
-        default_value: 0.0,
-        description: "Feed-forward scaling :math:`k_{ff}` of velocity"
-      }
-  constraints:
-    stopped_velocity_tolerance: {
       type: double,
-      default_value: 0.01,
-      description: "Velocity tolerance for at the end of the trajectory that indicates that controlled system is stopped.",
+      default_value: 0.0, # seconds
+      description: "Timeout after which the input command is considered stale.
+        Timeout is counted from the end of the trajectory (the last point).
+        ``cmd_timeout`` must be greater than ``constraints.goal_time``, otherwise ignored.
+        If zero, timeout is deactivated",
     }
-    goal_time: {
-      type: double,
-      default_value: 0.0,
-      description: "Time tolerance for achieving trajectory goal before or after commanded time.
-        If set to zero, the controller will wait a potentially infinite amount of time.",
-      validation: {
-        gt_eq: [0.0],
-      }
+  controller_plugin:
+    {
+      type: string,
+      default_value: "joint_trajectory_controller_plugins::PidTrajectoryPlugin",
+      description: "Type of the plugin for the trajectory controller",
+      read_only: true,
     }
-    __map_joints:
-      trajectory: {
+  constraints:
+    stopped_velocity_tolerance:
+      {
         type: double,
-        default_value: 0.0,
-        description: "Per-joint trajectory offset tolerance during movement.",
+        default_value: 0.01,
+        description: "Velocity tolerance for at the end of the trajectory that indicates that controlled system is stopped.",
       }
-      goal: {
+    goal_time:
+      {
         type: double,
         default_value: 0.0,
-        description: "Per-joint trajectory offset tolerance at the goal position.",
+        description:
+          "Time tolerance for achieving trajectory goal before or after commanded time.
+          If set to zero, the controller will wait a potentially infinite amount of time.",
+        validation: { gt_eq: [0.0] },
       }
+    __map_joints:
+      trajectory:
+        {
+          type: double,
+          default_value: 0.0,
+          description: "Per-joint trajectory offset tolerance during movement.",
+        }
+      goal:
+        {
+          type: double,
+          default_value: 0.0,
+          description: "Per-joint trajectory offset tolerance at the goal position.",
+        }
diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
index c112323430..1101450600 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller.cpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -469,30 +469,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_closed_loop_pid_adapter())
+  if (traj_controller_->use_external_control_law())
   {
-    EXPECT_EQ(pids.size(), 3);
-    auto gain_0 = pids.at(0)->get_gains();
-    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->compute_commands(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)->get_gains();
-    EXPECT_EQ(gain_0.p_gain_, kp);
+    pids->compute_commands(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();
@@ -819,8 +827,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
 
   if (traj_controller_->has_velocity_command_interface())
   {
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
+    // use_external_control_law_
+    if (traj_controller_->use_external_control_law())
     {
       // we expect u = k_p * (s_d-s) for positions
       EXPECT_NEAR(
@@ -946,8 +954,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
 
   if (traj_controller_->has_velocity_command_interface())
   {
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
+    // use_external_control_law_
+    if (traj_controller_->use_external_control_law())
     {
       // we expect u = k_p * (s_d-s) for joint0 and joint1
       EXPECT_NEAR(
@@ -1084,8 +1092,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
     EXPECT_LT(0.0, joint_vel_[1]);
     EXPECT_TRUE(std::isnan(current_command.velocities[2]));
 
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
+    if (traj_controller_->use_external_control_law())
     {
       // we expect u = k_p * (s_d-s)
       EXPECT_NEAR(
@@ -1198,8 +1205,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le
     EXPECT_LT(0.0, joint_vel_[1]);
     EXPECT_TRUE(std::isnan(current_command.velocities[2]));
 
-    // use_closed_loop_pid_adapter_
-    if (traj_controller_->use_closed_loop_pid_adapter())
+    if (traj_controller_->use_external_control_law())
     {
       // we expect u = k_p * (s_d-s)
       EXPECT_NEAR(
@@ -1367,9 +1373,9 @@ TEST_P(TrajectoryControllerTestParameterized, timeout)
 }
 
 /**
- * @brief check if use_closed_loop_pid is active
+ * @brief check if use_external_control_law is set
  */
-TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid)
+TEST_P(TrajectoryControllerTestParameterized, set_external_control_law)
 {
   rclcpp::executors::MultiThreadedExecutor executor;
 
@@ -1383,7 +1389,7 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid)
     (traj_controller_->has_effort_command_interface() &&
      !traj_controller_->has_position_command_interface()))
   {
-    EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter());
+    EXPECT_TRUE(traj_controller_->use_external_control_law());
   }
 }
 
@@ -1440,9 +1446,9 @@ TEST_P(TrajectoryControllerTestParameterized, velocity_error)
   }
   // is the velocity error correct?
   if (
-    traj_controller_->use_closed_loop_pid_adapter()  // always needed for PID controller
-    || (traj_controller_->has_velocity_state_interface() &&
-        traj_controller_->has_velocity_command_interface()))
+    traj_controller_->use_external_control_law() ||  // always needed for PID controller
+    (traj_controller_->has_velocity_state_interface() &&
+     traj_controller_->has_velocity_command_interface()))
   {
     // don't check against a value, because spline interpolation might overshoot depending on
     // interface combinations
@@ -1513,8 +1519,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order)
 
   if (traj_controller_->has_velocity_command_interface())
   {
-    // if use_closed_loop_pid_adapter_==false: we expect desired velocities from direct sampling
-    // if use_closed_loop_pid_adapter_==true: we expect desired velocities, because we use PID with
+    // if use_external_control_law_==false: we expect desired velocities from direct sampling
+    // if use_external_control_law_==true: we expect desired velocities, because we use PID with
     // feedforward term only
     EXPECT_GT(0.0, joint_vel_[0]);
     EXPECT_GT(0.0, joint_vel_[1]);
diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
index 1d4302fbc6..5a3bf9bc94 100644
--- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
+++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp
@@ -119,11 +119,6 @@ class TestableJointTrajectoryController
     }
   }
 
-  void set_joint_names(const std::vector<std::string> & joint_names)
-  {
-    params_.joints = joint_names;
-  }
-
   void set_command_joint_names(const std::vector<std::string> & command_joint_names)
   {
     command_joint_names_ = command_joint_names;
@@ -166,19 +161,23 @@ class TestableJointTrajectoryController
 
   bool has_effort_command_interface() const { return has_effort_command_interface_; }
 
-  bool use_closed_loop_pid_adapter() const { return use_closed_loop_pid_adapter_; }
+  bool use_external_control_law() const { return use_external_control_law_; }
 
   // START DEPRECATE
   bool is_open_loop() const { return params_.open_loop_control; }
   // END DEPRECATE
 
+  std::shared_ptr<joint_trajectory_controller_plugins::TrajectoryControllerBase> get_traj_contr()
+    const
+  {
+    return traj_contr_;
+  }
+
   joint_trajectory_controller::SegmentTolerances get_active_tolerances()
   {
     return *(active_tolerances_.readFromRT());
   }
 
-  std::vector<PidPtr> get_pids() const { return pids_; }
-
   joint_trajectory_controller::SegmentTolerances get_tolerances() const
   {
     return default_tolerances_;
@@ -297,12 +296,17 @@ class TrajectoryControllerTest : public ::testing::Test
       controller_name_, urdf, 100, "", traj_controller_->define_custom_node_options());
   }
 
+  /**
+   * @brief set PIDs for every entry in joint_names_
+   */
   void SetPidParameters(double p_value = 0.0, double ff_value = 1.0)
   {
     traj_controller_->trigger_declare_parameters();
     auto node = traj_controller_->get_node();
 
-    for (size_t i = 0; i < joint_names_.size(); ++i)
+    // if command_joints were not set manually, it was done in the on_configure() method
+    auto command_joint_names = node->get_parameter("command_joints").as_string_array();
+    for (size_t i = 0; i < command_joint_names.size(); ++i)
     {
       const std::string prefix = "gains." + joint_names_[i];
       const rclcpp::Parameter k_p(prefix + ".p", p_value);
@@ -338,10 +342,15 @@ class TrajectoryControllerTest : public ::testing::Test
     // read-only parameters have to be set before init -> won't be read otherwise
     SetUpTrajectoryController(executor, parameters_local, urdf);
 
-    // set pid parameters before configure
-    SetPidParameters(k_p, ff);
     traj_controller_->configure();
 
+    // set pid parameters before activate. The PID plugin has to be loaded already, otherwise
+    // parameters are not declared yet
+    if (traj_controller_->use_external_control_law())
+    {
+      SetPidParameters(k_p, ff);
+    }
+
     ActivateTrajectoryController(
       separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints,
       initial_eff_joints);
@@ -643,7 +652,7 @@ class TrajectoryControllerTest : public ::testing::Test
     // i.e., active but trivial trajectory (one point only)
     EXPECT_TRUE(traj_controller_->has_trivial_traj());
 
-    if (traj_controller_->use_closed_loop_pid_adapter() == false)
+    if (traj_controller_->use_external_control_law() == false)
     {
       if (traj_controller_->has_position_command_interface())
       {
diff --git a/joint_trajectory_controller_plugins/CMakeLists.txt b/joint_trajectory_controller_plugins/CMakeLists.txt
new file mode 100644
index 0000000000..11581906ad
--- /dev/null
+++ b/joint_trajectory_controller_plugins/CMakeLists.txt
@@ -0,0 +1,73 @@
+cmake_minimum_required(VERSION 3.8)
+project(joint_trajectory_controller_plugins LANGUAGES CXX)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+  add_compile_options(-Wall -Wextra -Wconversion)
+endif()
+
+# find dependencies
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_toolbox
+  generate_parameter_library
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  trajectory_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(backward_ros REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+generate_parameter_library(pid_trajectory_plugin_parameters
+  src/pid_trajectory_plugin_parameters.yaml
+)
+
+add_library(pid_trajectory_plugin SHARED src/pid_trajectory_plugin.cpp)
+add_library(joint_trajectory_controller_plugins::pid_trajectory_plugin ALIAS pid_trajectory_plugin)
+target_compile_features(pid_trajectory_plugin PUBLIC cxx_std_17)
+target_include_directories(pid_trajectory_plugin PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
+ament_target_dependencies(pid_trajectory_plugin PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(pid_trajectory_plugin PUBLIC
+  pid_trajectory_plugin_parameters
+)
+pluginlib_export_plugin_description_file(joint_trajectory_controller_plugins plugins.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include/${PROJECT_NAME}
+)
+install(
+  TARGETS pid_trajectory_plugin pid_trajectory_plugin_parameters
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  ament_add_gmock(test_load_pid_trajectory test/test_load_pid_trajectory.cpp)
+  target_link_libraries(test_load_pid_trajectory pid_trajectory_plugin)
+  ament_target_dependencies(test_load_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+  ament_add_gmock(test_pid_trajectory test/test_pid_trajectory.cpp)
+  target_link_libraries(test_pid_trajectory pid_trajectory_plugin)
+  ament_target_dependencies(test_pid_trajectory ${THIS_PACKAGE_INCLUDE_DEPENDS})
+endif()
+
+ament_export_include_directories(
+  "include/${PROJECT_NAME}"
+)
+ament_export_libraries(
+  pid_trajectory_plugin
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()
diff --git a/joint_trajectory_controller_plugins/doc/parameters_context.yaml b/joint_trajectory_controller_plugins/doc/parameters_context.yaml
new file mode 100644
index 0000000000..bd820d2578
--- /dev/null
+++ b/joint_trajectory_controller_plugins/doc/parameters_context.yaml
@@ -0,0 +1,15 @@
+gains: |
+  If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint.
+  This structure contains the controller gains for every joint with the control law
+
+  .. math:: u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v)
+
+  with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below),
+  the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively.
+
+  If the joint is of continuous type, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`,
+  i.e., the shortest rotation to the target position is the desired motion.
+  Otherwise  :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured
+  position :math:`s` from the state interface.
+
+  If you want to turn off the PIDs (open loop control), set the feedback gains to zero.
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
new file mode 100644
index 0000000000..1e9c3875c3
--- /dev/null
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp
@@ -0,0 +1,94 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "control_toolbox/pid.hpp"
+
+#include "joint_trajectory_controller_plugins/pid_trajectory_plugin_parameters.hpp"  // auto-generated by generate_parameter_library
+#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp"
+
+using PidPtr = std::shared_ptr<control_toolbox::Pid>;
+
+namespace joint_trajectory_controller_plugins
+{
+
+class PidTrajectoryPlugin : public TrajectoryControllerBase
+{
+public:
+  bool initialize(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    std::vector<size_t> map_cmd_to_joints) override;
+
+  bool configure() override;
+
+  bool activate() override;
+
+  bool compute_control_law_non_rt_impl(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+  {
+    // nothing to do
+    return true;
+  }
+
+  bool compute_control_law_rt_impl(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+  {
+    // nothing to do
+    return true;
+  }
+
+  bool update_gains_rt() override;
+
+  void compute_commands(
+    std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
+    const trajectory_msgs::msg::JointTrajectoryPoint error,
+    const trajectory_msgs::msg::JointTrajectoryPoint desired,
+    const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override;
+
+  void reset() override;
+
+  void start() override
+  {
+    // nothing to do
+  }
+
+protected:
+  /**
+   * @brief parse PID gains from parameter struct
+   */
+  void parse_gains();
+
+  // number of command joints
+  size_t num_cmd_joints_;
+  // map from joints in the message to command joints
+  std::vector<size_t> map_cmd_to_joints_;
+  // PID controllers
+  std::vector<PidPtr> pids_;
+  // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
+  std::vector<double> ff_velocity_scale_;
+
+  // Parameters from ROS for joint_trajectory_controller_plugins
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+};
+
+}  // namespace joint_trajectory_controller_plugins
+
+#endif  // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__PID_TRAJECTORY_PLUGIN_HPP_
diff --git a/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
new file mode 100644
index 0000000000..378a8f1869
--- /dev/null
+++ b/joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp
@@ -0,0 +1,168 @@
+// Copyright (c) 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
+#define JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "realtime_tools/realtime_buffer.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+
+namespace joint_trajectory_controller_plugins
+{
+class TrajectoryControllerBase
+{
+public:
+  TrajectoryControllerBase() = default;
+
+  virtual ~TrajectoryControllerBase() = default;
+
+  /**
+   * @brief initialize the controller plugin.
+   * declare parameters
+   * @param node the node handle to use for parameter handling
+   * @param map_cmd_to_joints a mapping from the joint names in the trajectory messages to the
+   * command joints
+   */
+  virtual bool initialize(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints) = 0;
+
+  /**
+   * @brief configure the controller plugin.
+   * parse read-only parameters, pre-allocate memory for the controller
+   */
+  virtual bool configure() = 0;
+
+  /**
+   * @brief activate the controller plugin.
+   * parse parameters
+   */
+  virtual bool activate() = 0;
+
+  /**
+   * @brief compute the control law for the given trajectory
+   *
+   * this method can take more time to compute the control law. Hence, it will block the execution
+   * of the trajectory until it finishes
+   *
+   * this method is not virtual, any overrides won't be called by JTC. Instead, override
+   * compute_control_law_non_rt_impl for your implementation
+   *
+   * @return true if the gains were computed, false otherwise
+   */
+  bool compute_control_law_non_rt(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
+  {
+    rt_control_law_ready_.writeFromNonRT(false);
+    auto ret = compute_control_law_non_rt_impl(trajectory);
+    rt_control_law_ready_.writeFromNonRT(true);
+    return ret;
+  }
+
+  /**
+   * @brief compute the control law for the given trajectory
+   *
+   * this method must finish quickly (within one controller-update rate)
+   *
+   * this method is not virtual, any overrides won't be called by JTC. Instead, override
+   * compute_control_law_rt_impl for your implementation
+   *
+   * @return true if the gains were computed, false otherwise
+   */
+  bool compute_control_law_rt(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
+  {
+    // TODO(christophfroehlich): Need a lock-free write here
+    rt_control_law_ready_.writeFromNonRT(false);
+    auto ret = compute_control_law_rt_impl(trajectory);
+    rt_control_law_ready_.writeFromNonRT(true);
+    return ret;
+  }
+
+  /**
+   * @brief called when the current trajectory started in update loop
+   *
+   * use this to implement switching of real-time buffers for updating the control law
+   */
+  virtual void start(void) = 0;
+
+  /**
+   * @brief update the gains from a RT thread
+   *
+   * this method must finish quickly (within one controller-update rate)
+   *
+   * @return true if the gains were updated, false otherwise
+   */
+  virtual bool update_gains_rt(void) = 0;
+
+  /**
+   * @brief compute the commands with the precalculated control law
+   *
+   * @param[out] tmp_command the output command
+   * @param[in] current the current state
+   * @param[in] error the error between the current state and the desired state
+   * @param[in] desired the desired state
+   * @param[in] duration_since_start the duration since the start of the trajectory
+   *            can be negative if the trajectory-start is in the future
+   * @param[in] period the period since the last update
+   */
+  virtual void compute_commands(
+    std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
+    const trajectory_msgs::msg::JointTrajectoryPoint error,
+    const trajectory_msgs::msg::JointTrajectoryPoint desired,
+    const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) = 0;
+
+  /**
+   * @brief reset internal states
+   *
+   * is called in on_deactivate() of JTC
+   */
+  virtual void reset() = 0;
+
+  /**
+   * @return true if the control law is ready (updated with the trajectory)
+   */
+  bool is_ready() { return rt_control_law_ready_.readFromRT(); }
+
+protected:
+  // the node handle for parameter handling
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+  // Are we computing the control law or is it valid?
+  realtime_tools::RealtimeBuffer<bool> rt_control_law_ready_;
+
+  /**
+   * @brief compute the control law from the given trajectory (in the non-RT loop)
+   * @return true if the gains were computed, false otherwise
+   */
+  virtual bool compute_control_law_non_rt_impl(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
+
+  /**
+   * @brief compute the control law for a single point (in the RT loop)
+   * @return true if the gains were computed, false otherwise
+   */
+  virtual bool compute_control_law_rt_impl(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
+};
+
+}  // namespace joint_trajectory_controller_plugins
+
+#endif  // JOINT_TRAJECTORY_CONTROLLER_PLUGINS__TRAJECTORY_CONTROLLER_BASE_HPP_
diff --git a/joint_trajectory_controller_plugins/package.xml b/joint_trajectory_controller_plugins/package.xml
new file mode 100644
index 0000000000..2d09f4beb7
--- /dev/null
+++ b/joint_trajectory_controller_plugins/package.xml
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>joint_trajectory_controller_plugins</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="christoph.froehlich@ait.ac.at">vscode</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>backward_ros</depend>
+  <depend>control_toolbox</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>pluginlib</depend>
+  <depend>trajectory_msgs</depend>
+
+  <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/joint_trajectory_controller_plugins/plugins.xml b/joint_trajectory_controller_plugins/plugins.xml
new file mode 100644
index 0000000000..af91db2a73
--- /dev/null
+++ b/joint_trajectory_controller_plugins/plugins.xml
@@ -0,0 +1,6 @@
+<library path="pid_trajectory_plugin">
+  <class
+  type="joint_trajectory_controller_plugins::PidTrajectoryPlugin" base_class_type="joint_trajectory_controller_plugins::TrajectoryControllerBase">
+    <description>A trajectory controller consisting of a PID controller per joint.</description>
+  </class>
+</library>
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
new file mode 100644
index 0000000000..4f596c58fb
--- /dev/null
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp
@@ -0,0 +1,151 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp"
+
+namespace joint_trajectory_controller_plugins
+{
+
+bool PidTrajectoryPlugin::initialize(
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints)
+{
+  node_ = node;
+  map_cmd_to_joints_ = map_cmd_to_joints;
+
+  try
+  {
+    // Create the parameter listener and get the parameters
+    param_listener_ = std::make_shared<ParamListener>(node_);
+  }
+  catch (const std::exception & e)
+  {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return false;
+  }
+
+  return true;
+}
+
+bool PidTrajectoryPlugin::configure()
+{
+  try
+  {
+    params_ = param_listener_->get_params();
+  }
+  catch (const std::exception & e)
+  {
+    fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what());
+    return false;
+  }
+
+  // parse read-only params
+  num_cmd_joints_ = params_.command_joints.size();
+  if (num_cmd_joints_ == 0)
+  {
+    RCLCPP_ERROR(node_->get_logger(), "[PidTrajectoryPlugin] No command joints specified.");
+    return false;
+  }
+  if (num_cmd_joints_ != map_cmd_to_joints_.size())
+  {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "[PidTrajectoryPlugin] map_cmd_to_joints has to be of size num_cmd_joints.");
+    return false;
+  }
+  pids_.resize(num_cmd_joints_);  // memory for the shared pointers, will be nullptr
+  // create the objects with default values
+  for (size_t i = 0; i < num_cmd_joints_; ++i)
+  {
+    pids_[i] = std::make_shared<control_toolbox::Pid>();
+  }
+  ff_velocity_scale_.resize(num_cmd_joints_, 0.0);
+
+  return true;
+}
+
+bool PidTrajectoryPlugin::activate()
+{
+  params_ = param_listener_->get_params();
+  parse_gains();
+  return true;
+}
+
+bool PidTrajectoryPlugin::update_gains_rt()
+{
+  if (param_listener_->is_old(params_))
+  {
+    params_ = param_listener_->get_params();
+    parse_gains();
+  }
+
+  return true;
+}
+
+void PidTrajectoryPlugin::parse_gains()
+{
+  for (size_t i = 0; i < num_cmd_joints_; ++i)
+  {
+    RCLCPP_DEBUG(
+      node_->get_logger(), "[PidTrajectoryPlugin] params_.command_joints %lu : %s", i,
+      params_.command_joints[i].c_str());
+
+    const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]);
+    pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp, true);
+    ff_velocity_scale_[i] = gains.ff_velocity_scale;
+
+    RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] gains.p: %f", gains.p);
+    RCLCPP_DEBUG(
+      node_->get_logger(), "[PidTrajectoryPlugin] ff_velocity_scale_: %f", ff_velocity_scale_[i]);
+  }
+
+  RCLCPP_INFO(
+    node_->get_logger(),
+    "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joint(s).",
+    num_cmd_joints_);
+}
+
+void PidTrajectoryPlugin::compute_commands(
+  std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/,
+  const trajectory_msgs::msg::JointTrajectoryPoint error,
+  const trajectory_msgs::msg::JointTrajectoryPoint desired,
+  const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period)
+{
+  // Update PIDs
+  for (auto i = 0ul; i < num_cmd_joints_; ++i)
+  {
+    tmp_command[map_cmd_to_joints_[i]] =
+      (desired.velocities[map_cmd_to_joints_[i]] * ff_velocity_scale_[i]) +
+      pids_[i]->compute_command(
+        error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]], period);
+  }
+}
+
+void PidTrajectoryPlugin::reset()
+{
+  for (const auto & pid : pids_)
+  {
+    if (pid)
+    {
+      pid->reset();
+    }
+  }
+}
+
+}  // namespace joint_trajectory_controller_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(
+  joint_trajectory_controller_plugins::PidTrajectoryPlugin,
+  joint_trajectory_controller_plugins::TrajectoryControllerBase)
diff --git a/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml
new file mode 100644
index 0000000000..f934fc9bdf
--- /dev/null
+++ b/joint_trajectory_controller_plugins/src/pid_trajectory_plugin_parameters.yaml
@@ -0,0 +1,37 @@
+joint_trajectory_controller_plugins:
+  command_joints: {
+    type: string_array,
+    default_value: [],
+    description: "Names of joints used by the controller plugin",
+    read_only: true,
+    validation: {
+      size_gt<>: [0],
+    }
+  }
+  gains:
+    __map_command_joints:
+      p: {
+        type: double,
+        default_value: 0.0,
+        description: "Proportional gain for PID"
+      }
+      i: {
+        type: double,
+        default_value: 0.0,
+        description: "Integral gain for PID"
+      }
+      d: {
+        type: double,
+        default_value: 0.0,
+        description: "Derivative gain for PID"
+      }
+      i_clamp: {
+        type: double,
+        default_value: 0.0,
+        description: "Integral clamp. Symmetrical in both positive and negative direction."
+      }
+      ff_velocity_scale: {
+        type: double,
+        default_value: 0.0,
+        description: "Feed-forward scaling of velocity."
+      }
diff --git a/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp
new file mode 100644
index 0000000000..c53c35c64a
--- /dev/null
+++ b/joint_trajectory_controller_plugins/test/test_load_pid_trajectory.cpp
@@ -0,0 +1,49 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <memory>
+#include <string>
+
+#include "gmock/gmock.h"
+
+#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+TEST(TestLoadPidController, load_controller)
+{
+  rclcpp::init(0, nullptr);
+
+  pluginlib::ClassLoader<joint_trajectory_controller_plugins::TrajectoryControllerBase>
+    traj_controller_loader(
+      "joint_trajectory_controller_plugins",
+      "joint_trajectory_controller_plugins::TrajectoryControllerBase");
+
+  std::shared_ptr<joint_trajectory_controller_plugins::TrajectoryControllerBase> traj_contr;
+
+  auto available_classes = traj_controller_loader.getDeclaredClasses();
+
+  std::cout << "available plugins:" << std::endl;
+  for (const auto & available_class : available_classes)
+  {
+    std::cout << "  " << available_class << std::endl;
+  }
+
+  std::string controller_type = "joint_trajectory_controller_plugins::PidTrajectoryPlugin";
+  ASSERT_TRUE(traj_controller_loader.isClassAvailable(controller_type));
+  ASSERT_NO_THROW(traj_contr = traj_controller_loader.createSharedInstance(controller_type));
+
+  rclcpp::shutdown();
+}
diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
new file mode 100644
index 0000000000..fd82e03111
--- /dev/null
+++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp
@@ -0,0 +1,110 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "test_pid_trajectory.hpp"
+
+TEST_F(PidTrajectoryTest, TestEmptySetup)
+{
+  std::shared_ptr<TestableJointTrajectoryControllerPlugin> traj_contr =
+    std::make_shared<TestableJointTrajectoryControllerPlugin>();
+
+  std::vector<size_t> map_cmd_to_joints{};
+  ASSERT_FALSE(traj_contr->initialize(node_, map_cmd_to_joints));
+}
+
+TEST_F(PidTrajectoryTest, TestSingleJoint)
+{
+  std::shared_ptr<TestableJointTrajectoryControllerPlugin> traj_contr =
+    std::make_shared<TestableJointTrajectoryControllerPlugin>();
+
+  std::vector<std::string> joint_names = {"joint1"};
+  auto joint_names_paramv = rclcpp::ParameterValue(joint_names);
+
+  // override parameter
+  node_->declare_parameter("command_joints", joint_names_paramv);
+
+  std::vector<size_t> map_cmd_to_joints{0};
+  ASSERT_TRUE(traj_contr->initialize(node_, map_cmd_to_joints));
+  node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
+  ASSERT_TRUE(traj_contr->configure());
+  ASSERT_TRUE(traj_contr->activate());
+  ASSERT_TRUE(traj_contr->compute_control_law_non_rt(
+    std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+  ASSERT_TRUE(traj_contr->update_gains_rt());
+
+  trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
+  traj_msg.positions.push_back(0.0);
+  traj_msg.velocities.push_back(0.0);
+  std::vector<double> tmp_command(1, 0.0);
+  const rclcpp::Duration time_since_start(1, 0);
+  const rclcpp::Duration period(1, 0);
+
+  ASSERT_NO_THROW(traj_contr->compute_commands(
+    tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
+}
+
+TEST_F(PidTrajectoryTest, TestMultipleJoints)
+{
+  std::shared_ptr<TestableJointTrajectoryControllerPlugin> traj_contr =
+    std::make_shared<TestableJointTrajectoryControllerPlugin>();
+
+  std::vector<std::string> joint_names = {"joint1", "joint2", "joint3"};
+  auto joint_names_paramv = rclcpp::ParameterValue(joint_names);
+
+  // override parameter
+  node_->declare_parameter("command_joints", joint_names_paramv);
+
+  std::vector<size_t> map_cmd_to_joints{0, 1, 2};
+  ASSERT_TRUE(traj_contr->initialize(node_, map_cmd_to_joints));
+  // set dynamic parameters
+  node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
+  node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 2.0));
+  node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 3.0));
+  ASSERT_TRUE(traj_contr->configure());
+  ASSERT_TRUE(traj_contr->activate());
+  ASSERT_TRUE(traj_contr->compute_control_law_non_rt(
+    std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+  ASSERT_TRUE(traj_contr->update_gains_rt());
+
+  ASSERT_TRUE(traj_contr->compute_control_law_non_rt(
+    std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+  ASSERT_TRUE(traj_contr->update_gains_rt());
+
+  trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
+  traj_msg.positions.push_back(1.0);
+  traj_msg.positions.push_back(1.0);
+  traj_msg.positions.push_back(1.0);
+  traj_msg.velocities.push_back(0.0);
+  traj_msg.velocities.push_back(0.0);
+  traj_msg.velocities.push_back(0.0);
+  std::vector<double> tmp_command(3, 0.0);
+  const rclcpp::Duration time_since_start(1, 0);
+  const rclcpp::Duration period(1, 0);
+
+  ASSERT_NO_THROW(traj_contr->compute_commands(
+    tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
+
+  EXPECT_EQ(tmp_command[0], 1.0);
+  EXPECT_EQ(tmp_command[1], 2.0);
+  EXPECT_EQ(tmp_command[2], 3.0);
+}
+
+int main(int argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}
diff --git a/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp
new file mode 100644
index 0000000000..ebe2b95a98
--- /dev/null
+++ b/joint_trajectory_controller_plugins/test/test_pid_trajectory.hpp
@@ -0,0 +1,72 @@
+// Copyright 2023 AIT Austrian Institute of Technology
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_PID_TRAJECTORY_HPP_
+#define TEST_PID_TRAJECTORY_HPP_
+
+#include <memory>
+#include <string>
+#include <thread>
+#include <vector>
+
+#include "gmock/gmock.h"
+
+#include "joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+
+namespace
+{
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_pid_trajectory");
+}  // namespace
+
+class TestableJointTrajectoryControllerPlugin
+: public joint_trajectory_controller_plugins::PidTrajectoryPlugin
+{
+public:
+  // updates mapped parameters, i.e., should be called after setting the joint names
+  void trigger_declare_parameters() { param_listener_->declare_params(); }
+};
+
+class PidTrajectoryTest : public ::testing::Test
+{
+public:
+  void SetUp() override
+  {
+    auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name();
+    node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(testname);
+    executor_->add_node(node_->get_node_base_interface());
+    executor_thread_ = std::thread([this]() { executor_->spin(); });
+  }
+
+  PidTrajectoryTest() { executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(); }
+
+  void TearDown() override
+  {
+    executor_->cancel();
+    if (executor_thread_.joinable())
+    {
+      executor_thread_.join();
+    }
+    node_.reset();
+  }
+
+protected:
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+  rclcpp::Executor::SharedPtr executor_;
+  std::thread executor_thread_;
+};
+
+#endif  // TEST_PID_TRAJECTORY_HPP_
diff --git a/ros2_controllers.cmake b/ros2_controllers.cmake
new file mode 100644
index 0000000000..eceacdf4d7
--- /dev/null
+++ b/ros2_controllers.cmake
@@ -0,0 +1,44 @@
+# Copyright 2025 AIT - Austrian Institute of Technology GmbH
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# set compiler options depending on detected compiler
+macro(set_compiler_options)
+  if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
+    add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable
+                        -Werror=return-type -Werror=shadow -Werror=format
+                        -Werror=missing-braces)
+    message(STATUS "Compiler warnings enabled for ${CMAKE_CXX_COMPILER_ID}")
+
+    # Extract major version if g++ is used
+    if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+
+      string(REPLACE "." ";" VERSION_LIST ${CMAKE_CXX_COMPILER_VERSION})
+      list(GET VERSION_LIST 0 GCC_MAJOR_VERSION)
+      list(GET VERSION_LIST 1 GCC_MINOR_VERSION)
+
+      message(STATUS "Detected GCC Version: ${CMAKE_CXX_COMPILER_VERSION} (Major: ${GCC_MAJOR_VERSION}, Minor: ${GCC_MINOR_VERSION})")
+
+      if (GCC_MAJOR_VERSION GREATER 10)
+        # GCC 11 introduced -Werror=range-loop-construct
+        add_compile_options(-Werror=range-loop-construct)
+      endif()
+    endif()
+  endif()
+endmacro()
+
+# using this instead of visibility macros
+# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
+macro(export_windows_symbols)
+  set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
+endmacro()