Chainable jtc

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -26,7 +26,7 @@
 #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 "controller_interface/chainable_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"
@@ -64,7 +64,7 @@ namespace joint_trajectory_controller
 {
 class Trajectory;
 
-class JointTrajectoryController : public controller_interface::ControllerInterface
+class JointTrajectoryController : public controller_interface::ChainableControllerInterface
 {
 public:
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
@@ -84,10 +84,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
-  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  controller_interface::return_type update(
-    const rclcpp::Time & time, const rclcpp::Duration & period) override;
-
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   controller_interface::CallbackReturn on_init() override;
 
@@ -116,6 +112,20 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     const rclcpp_lifecycle::State & previous_state) override;
 
 protected:
+  // internal reference values
+  std::vector<std::reference_wrapper<double>> position_reference_;
+  std::vector<std::reference_wrapper<double>> velocity_reference_;
+  std::vector<std::reference_wrapper<double>> acceleration_reference_;
+  std::vector<std::reference_wrapper<double>> effort_reference_;
+  std::vector<std::string> joint_names_;
+  std::vector<double> last_references_ = {};
+  bool first_time_ = true;
+  rclcpp::Time last_time_;
+
+  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
+
+  controller_interface::return_type update_reference_from_subscribers(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
   // To reduce number of variables and to make the code shorter the interfaces are ordered in types
   // as the following constants
   const std::vector<std::string> allowed_interface_types_ = {
@@ -125,6 +135,26 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     hardware_interface::HW_IF_EFFORT,
   };
 
+  bool reference_changed();
+
+  template <typename M>
+  void copy_reference_interfaces_values(
+    std::vector<M> & msg_container, std::vector<std::reference_wrapper<M>> & reference_input);
+
+  /**
+   * If controller is in chained mode we create new JointTrajectory msgs
+   * from the reference interface input.
+   * If controller is NOT in chained mode we get the JointTrajectory msg
+   * from topic.
+   * 
+   * For detailed explanation on this have a look at the update_reference_from_subscribers
+   * method which normally would handle such task
+  */
+  void update_joint_trajectory_point_from_input(const rclcpp::Time & time);
+
+  controller_interface::return_type update_and_write_commands(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
   // Preallocate variables used in the realtime update() function
   trajectory_msgs::msg::JointTrajectoryPoint state_current_;
   trajectory_msgs::msg::JointTrajectoryPoint command_current_;

joint_trajectory_controller/joint_trajectory_plugin.xml

@@ -1,5 +1,5 @@
 <library path="joint_trajectory_controller">
-  <class name="joint_trajectory_controller/JointTrajectoryController" type="joint_trajectory_controller::JointTrajectoryController" base_class_type="controller_interface::ControllerInterface">
+  <class name="joint_trajectory_controller/JointTrajectoryController" type="joint_trajectory_controller::JointTrajectoryController" base_class_type="controller_interface::ChainableControllerInterface">
   <description>
     The joint trajectory controller executes joint-space trajectories on a set of joints
   </description>