Pass scaling factor to plugin

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -335,10 +335,12 @@ controller_interface::return_type JointTrajectoryController::update(
       {
         if (traj_contr_)
         {
+          double scaling_fact = scaling_factor_.load();
           traj_contr_->compute_commands(
-            tmp_command_, state_current_, state_error_, command_next_,
+            tmp_command_, scaling_fact, state_current_, state_error_, command_next_,
             traj_ctr_state_interfaces_values_, time - current_trajectory_->time_from_start(),
             period);
+          scaling_factor_.store(scaling_fact);
         }
 
         // set values for next hardware write()

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -487,15 +487,20 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters)
     rclcpp::Duration duration_since_start(std::chrono::milliseconds(250));
     rclcpp::Duration period(std::chrono::milliseconds(100));
 
-    pids->compute_commands(tmp_command, current, error, desired, {}, duration_since_start, period);
+    double scaling_fact = 1.0;
+    pids->compute_commands(
+      tmp_command, scaling_fact, current, error, desired, {}, duration_since_start, period);
     EXPECT_EQ(tmp_command.at(0), 0.0);
+    EXPECT_EQ(scaling_fact, 1.0);
 
     double kp = 1.0;
     SetPidParameters(kp);
     updateControllerAsync();
 
-    pids->compute_commands(tmp_command, current, error, desired, {}, duration_since_start, period);
+    pids->compute_commands(
+      tmp_command, scaling_fact, current, error, desired, {}, duration_since_start, period);
     EXPECT_EQ(tmp_command.at(0), 1.0);
+    EXPECT_EQ(scaling_fact, 1.0);
   }
   else
   {

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp

@@ -35,7 +35,8 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
   bool update_gains_rt() override;
 
   void compute_commands(
-    std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
+    std::vector<double> & tmp_command, double & scaling_fact,
+    const trajectory_msgs::msg::JointTrajectoryPoint current,
     const trajectory_msgs::msg::JointTrajectoryPoint error,
     const trajectory_msgs::msg::JointTrajectoryPoint desired,
     const std::vector<double> & opt_state_interfaces_values,

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp

@@ -79,10 +79,10 @@ class TrajectoryControllerBase
    */
   bool compute_control_law_non_rt(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory,
-    const std::vector<double> & opt_state_interfaces_values_)
+    const std::vector<double> & opt_state_interfaces_values)
   {
     rt_control_law_ready_ = false;
-    auto ret = on_compute_control_law_non_rt(trajectory, opt_state_interfaces_values_);
+    auto ret = on_compute_control_law_non_rt(trajectory, opt_state_interfaces_values);
     rt_control_law_ready_ = true;
     return ret;
   }
@@ -99,10 +99,10 @@ class TrajectoryControllerBase
    */
   bool compute_control_law_rt(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory,
-    const std::vector<double> & opt_state_interfaces_values_)
+    const std::vector<double> & opt_state_interfaces_values)
   {
     rt_control_law_ready_ = false;
-    auto ret = on_compute_control_law_rt(trajectory, opt_state_interfaces_values_);
+    auto ret = on_compute_control_law_rt(trajectory, opt_state_interfaces_values);
     rt_control_law_ready_ = true;
     return ret;
   }
@@ -127,6 +127,7 @@ class TrajectoryControllerBase
    * @brief compute the commands with the precalculated control law
    *
    * @param[out] tmp_command the output command
+   * @param[out] scaling_fact scaling factor if ctrl is not following reference
    * @param[in] current the current state
    * @param[in] error the error between the current state and the desired state
    * @param[in] desired the desired state
@@ -137,7 +138,8 @@ class TrajectoryControllerBase
    * @param[in] period the period since the last update
    */
   virtual void compute_commands(
-    std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
+    std::vector<double> & tmp_command, double & scaling_fact,
+    const trajectory_msgs::msg::JointTrajectoryPoint current,
     const trajectory_msgs::msg::JointTrajectoryPoint error,
     const trajectory_msgs::msg::JointTrajectoryPoint desired,
     const std::vector<double> & opt_state_interfaces_values,

joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp

@@ -113,7 +113,8 @@ void PidTrajectoryPlugin::parse_gains()
 }
 
 void PidTrajectoryPlugin::compute_commands(
-  std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/,
+  std::vector<double> & tmp_command, double & /*scaling_fact*/,
+  const trajectory_msgs::msg::JointTrajectoryPoint /*current*/,
   const trajectory_msgs::msg::JointTrajectoryPoint error,
   const trajectory_msgs::msg::JointTrajectoryPoint desired,
   const std::vector<double> & /* opt_state_interfaces_values*/,

joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp

@@ -50,8 +50,10 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
   const rclcpp::Duration time_since_start(1, 0);
   const rclcpp::Duration period(1, 0);
 
+  double scaling_fact = 1.0;
   ASSERT_NO_THROW(traj_contr->compute_commands(
-    tmp_command, traj_msg, traj_msg, traj_msg, {}, time_since_start, period));
+    tmp_command, scaling_fact, traj_msg, traj_msg, traj_msg, {}, time_since_start, period));
+  EXPECT_EQ(scaling_fact, 1.0);
 }
 
 TEST_F(PidTrajectoryTest, TestMultipleJoints)
@@ -92,8 +94,10 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
   const rclcpp::Duration time_since_start(1, 0);
   const rclcpp::Duration period(1, 0);
 
+  double scaling_fact = 1.0;
   ASSERT_NO_THROW(traj_contr->compute_commands(
-    tmp_command, traj_msg, traj_msg, traj_msg, {}, time_since_start, period));
+    tmp_command, scaling_fact, traj_msg, traj_msg, traj_msg, {}, time_since_start, period));
+  EXPECT_EQ(scaling_fact, 1.0);
 
   EXPECT_EQ(tmp_command[0], 1.0);
   EXPECT_EQ(tmp_command[1], 2.0);