[JTC] Move PID to controller plugin

joint_trajectory_controller/CMakeLists.txt

@@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rsl
   tl_expected
   trajectory_msgs
+  joint_trajectory_controller_plugins
 )
 
 find_package(ament_cmake REQUIRED)
@@ -104,4 +105,5 @@ install(TARGETS
 
 ament_export_targets(export_joint_trajectory_controller HAS_LIBRARY_TARGET)
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 ament_package()

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -25,12 +25,9 @@
 #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/visibility_control.h"
+#include "pluginlib/class_loader.hpp"
 #include "rclcpp/duration.hpp"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp/time.hpp"
@@ -45,8 +42,11 @@
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 
-// auto-generated by generate_parameter_library
-#include "joint_trajectory_controller_parameters.hpp"
+#include "joint_trajectory_controller/interpolation_methods.hpp"
+#include "joint_trajectory_controller/tolerances.hpp"
+#include "joint_trajectory_controller/visibility_control.h"
+#include "joint_trajectory_controller_parameters.hpp"  // auto-generated by generate_parameter_library
+#include "joint_trajectory_controller_plugins/trajectory_controller_base.hpp"
 
 using namespace std::chrono_literals;  // NOLINT
 
@@ -134,6 +134,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
   // Storing command joint names for interfaces
   std::vector<std::string> command_joint_names_;
+  // TODO(anyone) remove this if there is another way to lock command_joints parameter
+  rclcpp_lifecycle::LifecycleNode::PreSetParametersCallbackHandle::SharedPtr lock_cmd_joint_names;
 
   // Parameters from ROS for joint_trajectory_controller
   std::shared_ptr<ParamListener> param_listener_;
@@ -162,11 +164,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 position error is wrapped around
   std::vector<bool> joints_angle_wraparound_;
   // reserved storage for result of the command when closed loop pid adapter is used
@@ -233,7 +237,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool validate_trajectory_msg(const trajectory_msgs::msg::JointTrajectory & trajectory) const;
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  void add_new_trajectory_msg(
+  void add_new_trajectory_msg_nonRT(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
+  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
+  void add_new_trajectory_msg_RT(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & traj_msg);
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   bool validate_trajectory_point_field(
@@ -285,8 +292,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);
 

joint_trajectory_controller/package.xml

@@ -30,6 +30,7 @@
   <depend>rsl</depend>
   <depend>tl_expected</depend>
   <depend>trajectory_msgs</depend>
+  <depend>joint_trajectory_controller_plugins</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>