[JTC] Proposal for custom "controller" plugins

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()

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

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.

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);
 

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>