From 6981a3ea67dd6814d12c467e581404da1d8b1fcb Mon Sep 17 00:00:00 2001
From: Lennart Nachtigall <lennart.nachtigall@sci-mo.de>
Date: Wed, 8 Nov 2023 10:34:23 +0100
Subject: [PATCH] added option for setting speedscaling factor via topic

formatting

Allow changing topic name, made topic transient_local
---
 .../scaled_joint_trajectory_controller.hpp    |  8 ++++++-
 .../scaled_joint_trajectory_controller.cpp    | 24 ++++++++++++++-----
 ...oint_trajectory_controller_parameters.yaml | 10 ++++++++
 3 files changed, 35 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..344cf1e94 100644
--- a/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
+++ b/ur_controllers/include/ur_controllers/scaled_joint_trajectory_controller.hpp
@@ -45,11 +45,15 @@
 #include "rclcpp/duration.hpp"
 #include "scaled_joint_trajectory_controller_parameters.hpp"
 
+#include <std_msgs/msg/float64.hpp>
+
 namespace ur_controllers
 {
 class ScaledJointTrajectoryController : public joint_trajectory_controller::JointTrajectoryController
 {
 public:
+  using ScalingFactorMsg = std_msgs::msg::Float64;
+
   ScaledJointTrajectoryController() = default;
   ~ScaledJointTrajectoryController() override = default;
 
@@ -73,11 +77,13 @@ class ScaledJointTrajectoryController : public joint_trajectory_controller::Join
   };
 
 private:
-  double scaling_factor_{};
+  double scaling_factor_{ 1.0 };
   realtime_tools::RealtimeBuffer<TimeData> time_data_;
 
   std::shared_ptr<scaled_joint_trajectory_controller::ParamListener> scaled_param_listener_;
   scaled_joint_trajectory_controller::Params scaled_params_;
+
+  rclcpp::Subscription<ScalingFactorMsg>::SharedPtr scaling_factor_sub_;
 };
 }  // namespace ur_controllers
 
diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp
index e7cee27de..bd0451510 100644
--- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp
+++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp
@@ -58,13 +58,23 @@ 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_.use_speed_scaling_topic_instead)
+    conf.names.push_back(scaled_params_.speed_scaling_interface_name);
 
   return conf;
 }
 
 controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state)
 {
+  if (scaled_params_.use_speed_scaling_topic_instead) {
+    auto qos = rclcpp::QoS(10);
+    qos.transient_local();
+
+    scaling_factor_sub_ = get_node()->create_subscription<ScalingFactorMsg>(
+        scaled_params_.speed_scaling_topic_name, qos,
+        [&](const ScalingFactorMsg& msg) { scaling_factor_ = std::clamp(msg.data / 100.0, 0.0, 1.0); });
+  }
+
   TimeData time_data;
   time_data.time = get_node()->now();
   time_data.period = rclcpp::Duration::from_nanoseconds(0);
@@ -76,11 +86,13 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat
 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 (!scaled_params_.use_speed_scaling_topic_instead) {
+    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 (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
diff --git a/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml b/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml
index e72b0e58c..0e7785bb4 100644
--- a/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml
+++ b/ur_controllers/src/scaled_joint_trajectory_controller_parameters.yaml
@@ -4,3 +4,13 @@ scaled_joint_trajectory_controller:
       default_value: "speed_scaling/speed_scaling_factor",
       description: "Fully qualified name of the speed scaling interface name"
     }
+    use_speed_scaling_topic_instead: {
+      type: bool,
+      default_value: false,
+      description: "Instead of using the speed_scaling_interface_name listen on <speed_scaling_topic_name>"
+    }
+    speed_scaling_topic_name: {
+      type: string,
+      default_value: "~/speed_scaling_factor",
+      description: "Topic name for the speed scaling factor (if enabled)"
+    }