[JTC] Proposal for custom "controller" plugins

joint_trajectory_controller/CMakeLists.txt

@@ -12,6 +12,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   controller_interface
   generate_parameter_library
   hardware_interface
+  joint_trajectory_controller_plugins
   pluginlib
   rclcpp
   rclcpp_lifecycle
@@ -107,4 +108,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

@@ -24,13 +24,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/trajectory.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 +41,12 @@
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 #include "urdf/model.h"
 
-// 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/trajectory.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
 
@@ -123,6 +123,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_;
@@ -151,11 +153,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_;
@@ -237,7 +241,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(
@@ -288,8 +295,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

@@ -13,14 +13,15 @@
 
   <depend>angles</depend>
   <depend>backward_ros</depend>
-  <depend>controller_interface</depend>
   <depend>control_msgs</depend>
   <depend>control_toolbox</depend>
+  <depend>controller_interface</depend>
   <depend>generate_parameter_library</depend>
   <depend>hardware_interface</depend>
+  <depend>joint_trajectory_controller_plugins</depend>
   <depend>pluginlib</depend>
-  <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>rclcpp</depend>
   <depend>realtime_tools</depend>
   <depend>rsl</depend>
   <depend>tl_expected</depend>