Replace SJTC implementation with upstream (#1485)

Since speed scaling is merged in the upstream JTC, we dont't need to keep
our SJTC implementation around any longer. For backwards comaptibility we
might want to keep it a little longer as the parameters aren't exactly the
same.

Author: urfeex

ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp

@@ -37,13 +37,8 @@
 #ifndef UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
 #define UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
 
-#include <optional>
 #include <memory>
-#include <vector>
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "rclcpp/time.hpp"
-#include "rclcpp/duration.hpp"
 #include "ur_controllers/scaled_joint_trajectory_controller_parameters.hpp"
 
 namespace ur_controllers
@@ -54,43 +49,11 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
   ScaledJointTrajectoryController() = default;
   ~ScaledJointTrajectoryController() override = default;
 
-  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
-
-  controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
-
-  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
-
   CallbackReturn on_init() override;
 
 private:
-  std::atomic<double> scaling_factor_{ 1.0 };
-
-  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> scaling_state_interface_ =
-      std::nullopt;
-
   std::shared_ptr<scaled_joint_trajectory_controller::ParamListener> scaled_param_listener_;
   scaled_joint_trajectory_controller::Params scaled_params_;
-
-  // Private methods copied from Upstream JTC
-  void update_pids();
-
-  /**
-   * @brief Assigns the values from a trajectory point interface to a joint interface.
-   *
-   * @tparam T The type of the joint interface.
-   * @param[out] joint_interface The reference_wrapper to assign the values to
-   * @param[in] trajectory_point_interface Containing the values to assign.
-   * @todo: Use auto in parameter declaration with c++20
-   */
-  template <typename T>
-  bool assign_interface_from_point(const T& joint_interface, const std::vector<double>& trajectory_point_interface)
-  {
-    bool success = true;
-    for (size_t index = 0; index < num_cmd_joints_; ++index) {
-      success &= joint_interface[index].get().set_value(trajectory_point_interface[map_cmd_to_joints_[index]]);
-    }
-    return success;
-  }
 };
 }  // namespace ur_controllers