Add scaling factor to JTC

This adds a scaling factor between 0 and 1 to the JTC so that the trajectory
time inside the controller is extended respectively. A value of 0.5 means
that trajectory execution will take twice as long as the trajectory states.

The scaling factor itself is read from the hardware for now.

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -64,6 +64,16 @@ namespace joint_trajectory_controller
 {
 class Trajectory;
 
+struct TimeData
+{
+  TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0)
+  {
+  }
+  rclcpp::Time time;
+  rclcpp::Duration period;
+  rclcpp::Time uptime;
+};
+
 class JointTrajectoryController : public controller_interface::ControllerInterface
 {
 public:
@@ -174,6 +184,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // reserved storage for result of the command when closed loop pid adapter is used
   std::vector<double> tmp_command_;
 
+  // Things around speed scaling
+  double scaling_factor_{};
+  realtime_tools::RealtimeBuffer<TimeData> time_data_;
+
   // Timeout to consider commands old
   double cmd_timeout_;
   // Are we holding position?

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -17,6 +17,7 @@
 #include <stddef.h>
 #include <chrono>
 #include <functional>
+#include <iostream>
 #include <memory>
 #include <ostream>
 #include <ratio>
@@ -114,12 +115,20 @@ JointTrajectoryController::state_interface_configuration() const
       conf.names.push_back(joint_name + "/" + interface_type);
     }
   }
+  conf.names.push_back(params_.speed_scaling_interface_name);
   return conf;
 }
 
 controller_interface::return_type JointTrajectoryController::update(
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
+  if (state_interfaces_.back().get_name() == 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.",
+                 params_.speed_scaling_interface_name.c_str());
+  }
+
   if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
   {
     return controller_interface::return_type::OK;
@@ -189,25 +198,34 @@ controller_interface::return_type JointTrajectoryController::update(
   // currently carrying out a trajectory
   if (has_active_trajectory())
   {
+    // Adjust time with scaling factor
+    TimeData time_data;
+    time_data.time = time;
+    rcl_duration_value_t t_period = (time_data.time - time_data_.readFromRT()->time).nanoseconds();
+    time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * t_period);
+    time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
+    rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period);
+    time_data_.writeFromNonRT(time_data);
+
     bool first_sample = false;
     // if sampling the first time, set the point before you sample
     if (!traj_external_point_ptr_->is_sampled_already())
     {
       first_sample = true;
       if (params_.open_loop_control)
       {
-        traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_);
+        traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_);
       }
       else
       {
-        traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_);
+        traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_);
       }
     }
 
     // find segment for current timestamp
     TrajectoryPointConstIter start_segment_itr, end_segment_itr;
     const bool valid_point = traj_external_point_ptr_->sample(
-      time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
+      traj_time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
 
     if (valid_point)
     {
@@ -934,6 +952,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
 controller_interface::CallbackReturn JointTrajectoryController::on_activate(
   const rclcpp_lifecycle::State &)
 {
+  // Setup time_data buffer used for scaling
+  TimeData time_data;
+  time_data.time = get_node()->now();
+  time_data.period = rclcpp::Duration::from_nanoseconds(0);
+  time_data.uptime = get_node()->now();
+  time_data_.initRT(time_data);
+
   // order all joints in the storage
   for (const auto & interface : params_.command_interfaces)
   {

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -39,6 +39,11 @@ joint_trajectory_controller:
       "joint_trajectory_controller::state_interface_type_combinations": null,
     }
   }
+  speed_scaling_interface_name: {
+    type: string,
+    default_value: "speed_scaling/speed_scaling_factor",
+    description: "Fully qualified name of the speed scaling interface name"
+  }
   allow_partial_joints_goal: {
     type: bool,
     default_value: false,