Use a subscriber instead of a service

Author: Felix Exner

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -23,7 +23,6 @@
 #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_msgs/srv/set_scaling_factor.hpp"
 #include "control_toolbox/pid.hpp"
 #include "controller_interface/controller_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
@@ -43,6 +42,8 @@
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 
+#include "std_msgs/msg/float64.hpp"
+
 // auto-generated by generate_parameter_library
 #include "joint_trajectory_controller_parameters.hpp"
 
@@ -169,8 +170,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   std::vector<double> tmp_command_;
 
   // Things around speed scaling
-  double scaling_factor_{};
   realtime_tools::RealtimeBuffer<TimeData> time_data_;
+  double scaling_factor_{1.0};
 
   // Timeout to consider commands old
   double cmd_timeout_;
@@ -312,14 +313,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void resize_joint_trajectory_point_command(
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
 
-  bool set_scaling_factor(
-    control_msgs::srv::SetScalingFactor::Request::SharedPtr req,
-    control_msgs::srv::SetScalingFactor::Response::SharedPtr resp);
+  bool set_scaling_factor(const double scaling_factor);
 
   urdf::Model model_;
 
   realtime_tools::RealtimeBuffer<double> scaling_factor_rt_buff_;
-  rclcpp::Service<control_msgs::srv::SetScalingFactor>::SharedPtr set_scaling_factor_srv_;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr scaling_factor_sub_;
 
   /**
    * @brief Assigns the values from a trajectory point interface to a joint interface.

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -914,10 +914,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     !has_velocity_command_interface_ && !has_acceleration_command_interface_ &&
     !has_effort_command_interface_)
   {
-    set_scaling_factor_srv_ = get_node()->create_service<control_msgs::srv::SetScalingFactor>(
-      "~/set_scaling_factor", std::bind(
-                                &JointTrajectoryController::set_scaling_factor, this,
-                                std::placeholders::_1, std::placeholders::_2));
+    scaling_factor_sub_ = get_node()->create_subscription<std_msgs::msg::Float64>(
+      "~/speed_scaling_input", rclcpp::SystemDefaultsQoS(),
+      [&](const std_msgs::msg::Float64 & msg) { set_scaling_factor(msg.data);});
     RCLCPP_INFO(
       logger, "Setting initial scaling factor to %2f", params_.scaling_factor_initial_default);
     scaling_factor_rt_buff_.writeFromNonRT(params_.scaling_factor_initial_default);
@@ -1632,19 +1631,16 @@ void JointTrajectoryController::resize_joint_trajectory_point_command(
   }
 }
 
-bool JointTrajectoryController::set_scaling_factor(
-  control_msgs::srv::SetScalingFactor::Request::SharedPtr req,
-  control_msgs::srv::SetScalingFactor::Response::SharedPtr resp)
+bool JointTrajectoryController::set_scaling_factor(const double scaling_factor)
 {
-  if (req->scaling_factor < 0 && req->scaling_factor > 1)
+  if (scaling_factor < 0 || scaling_factor > 1)
   {
     RCLCPP_WARN(
       get_node()->get_logger(), "Scaling factor has to be in range [0, 1]. Ignoring input!");
-    resp->success = false;
-    return true;
+    return false;
   }
 
-  RCLCPP_INFO(get_node()->get_logger(), "New scaling factor will be %f", req->scaling_factor);
+  RCLCPP_INFO(get_node()->get_logger(), "New scaling factor will be %f", scaling_factor);
   if (params_.speed_scaling_command_interface_name.empty())
   {
     if (!params_.speed_scaling_state_interface_name.empty())
@@ -1655,7 +1651,7 @@ bool JointTrajectoryController::set_scaling_factor(
         "This will likely get overwritten by the hardware again. If available, please also setup "
         "the speed_scaling_command_interface_name");
     }
-    scaling_factor_rt_buff_.writeFromNonRT(req->scaling_factor);
+    scaling_factor_rt_buff_.writeFromNonRT(scaling_factor);
   }
   else
   {
@@ -1665,12 +1661,11 @@ bool JointTrajectoryController::set_scaling_factor(
       {
         if (interface.get_name() == params_.speed_scaling_command_interface_name)
         {
-          interface.set_value(static_cast<double>(req->scaling_factor));
+          interface.set_value(static_cast<double>(scaling_factor));
         }
       }
     }
   }
-  resp->success = true;
   return true;
 }