From 39ff78f20c1c8c781a8492f0c7ac36c8cf1a749f Mon Sep 17 00:00:00 2001
From: "Felix Exner (fexner)" <exner@fzi.de>
Date: Mon, 14 Oct 2024 17:02:00 +0200
Subject: [PATCH] [SJTC] Make scaling interface optional (#1145)

* [SJTC] Make scaling interface optional

This way, the controller can be used on systems, where no scaling
interface is available (e.g. GZ). The upstream version in ros2_controllers
will have the same behavior.

* Move logging to function that's only called once
---
 .../scaled_joint_trajectory_controller.hpp    |  7 ++++-
 .../scaled_joint_trajectory_controller.cpp    | 31 +++++++++++++++----
 2 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
index 75a450547..58bd12426 100644
--- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
+++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
@@ -37,6 +37,8 @@
 #ifndef UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
 #define UR_CONTROLLERS__SCALED_JOINT_TRAJECTORY_CONTROLLER_HPP_
 
+#include <optional>
+#include <memory>
 #include "angles/angles.h"
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
@@ -73,9 +75,12 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
   };
 
 private:
-  double scaling_factor_{};
+  double scaling_factor_{ 1.0 };
   realtime_tools::RealtimeBuffer<TimeData> time_data_;
 
+  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_;
 };
diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp
index 27303c1ea..0fdb5c6c6 100644
--- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp
+++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp
@@ -50,6 +50,12 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_init()
   // Create the parameter listener and get the parameters
   scaled_param_listener_ = std::make_shared<scaled_joint_trajectory_controller::ParamListener>(get_node());
   scaled_params_ = scaled_param_listener_->get_params();
+  if (!scaled_params_.speed_scaling_interface_name.empty()) {
+    RCLCPP_INFO(get_node()->get_logger(), "Using scaling state from the hardware from interface %s.",
+                scaled_params_.speed_scaling_interface_name.c_str());
+  } else {
+    RCLCPP_INFO(get_node()->get_logger(), "No scaling interface set. This controller will not use speed scaling.");
+  }
 
   return JointTrajectoryController::on_init();
 }
@@ -58,7 +64,10 @@ controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::st
 {
   controller_interface::InterfaceConfiguration conf;
   conf = JointTrajectoryController::state_interface_configuration();
-  conf.names.push_back(scaled_params_.speed_scaling_interface_name);
+
+  if (!scaled_params_.speed_scaling_interface_name.empty()) {
+    conf.names.push_back(scaled_params_.speed_scaling_interface_name);
+  }
 
   return conf;
 }
@@ -70,17 +79,27 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat
   time_data.period = rclcpp::Duration::from_nanoseconds(0);
   time_data.uptime = get_node()->now();
   time_data_.initRT(time_data);
+
+  // Set scaling interfaces
+  if (!scaled_params_.speed_scaling_interface_name.empty()) {
+    auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), [&](auto& interface) {
+      return (interface.get_name() == scaled_params_.speed_scaling_interface_name);
+    });
+    if (it != state_interfaces_.end()) {
+      scaling_state_interface_ = *it;
+    } else {
+      RCLCPP_ERROR(get_node()->get_logger(), "Did not find speed scaling interface in state interfaces.");
+    }
+  }
+
   return JointTrajectoryController::on_activate(state);
 }
 
 controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time,
                                                                           const rclcpp::Duration& period)
 {
-  if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) {
-    scaling_factor_ = state_interfaces_.back().get_value();
-  } else {
-    RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.",
-                 scaled_params_.speed_scaling_interface_name.c_str());
+  if (scaling_state_interface_.has_value()) {
+    scaling_factor_ = scaling_state_interface_->get().get_value();
   }
 
   if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {