Remove local vars and just call compute-commands with duration in trajectory

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -223,7 +223,7 @@ controller_interface::return_type JointTrajectoryController::update(
       if (traj_contr_)
       {
         // set start time of trajectory to traj_contr_
-        traj_contr_->start(time, traj_external_point_ptr_->time_from_start());
+        traj_contr_->start();
       }
     }
 
@@ -300,7 +300,8 @@ controller_interface::return_type JointTrajectoryController::update(
         if (traj_contr_)
         {
           traj_contr_->computeCommands(
-            tmp_command_, state_current_, state_error_, state_desired_, time, period);
+            tmp_command_, state_current_, state_error_, state_desired_,
+            time - traj_external_point_ptr_->time_from_start(), period);
         }
 
         // set values for next hardware write()

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp

@@ -40,18 +40,27 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
   bool activate() override;
 
   bool computeControlLaw(
-    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override;
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+  {
+    // nothing to do
+    return true;
+  }
 
   bool updateGainsRT() override;
 
   void computeCommands(
     std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
     const trajectory_msgs::msg::JointTrajectoryPoint error,
-    const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time,
-    const rclcpp::Duration & period) override;
+    const trajectory_msgs::msg::JointTrajectoryPoint desired,
+    const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) override;
 
   void reset() override;
 
+  void start() override
+  {
+    // nothing to do
+  }
+
 protected:
   /**
    * @brief parse PID gains from parameter struct

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp

@@ -82,22 +82,11 @@ class TrajectoryControllerBase
   }
 
   /**
-   * @brief set the time when the current trajectory started
+   * @brief called when the current trajectory started in update loop
    *
-   * (same logic as trajectory class)
+   * use this to implement switching of real-time buffers for updating the control law
    */
-  void start(const rclcpp::Time time, const rclcpp::Time trajectory_start_time)
-  {
-    if (trajectory_start_time.seconds() == 0.0)
-    {
-      trajectory_start_time_ = time;
-    }
-    else
-    {
-      trajectory_start_time_ = trajectory_start_time;
-    }
-    start_trajectory_ = true;
-  }
+  virtual void start(void) = 0;
 
   /**
    * @brief update the gains from a RT thread
@@ -110,14 +99,22 @@ class TrajectoryControllerBase
   virtual bool updateGainsRT(void) = 0;
 
   /**
-   * @brief compute the commands with the calculated gains
+   * @brief compute the commands with the precalculated control law
+   *
+   * @param[out] tmp_command the output command
+   * @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
+   * @param[in] duration_since_start the duration since the start of the trajectory
+   *            can be negative if the trajectory-start is in the future
+   * @param[in] period the period since the last update
    */
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
   virtual void computeCommands(
     std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
     const trajectory_msgs::msg::JointTrajectoryPoint error,
-    const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & time,
-    const rclcpp::Duration & period) = 0;
+    const trajectory_msgs::msg::JointTrajectoryPoint desired,
+    const rclcpp::Duration & duration_since_start, const rclcpp::Duration & period) = 0;
 
   /**
    * @brief reset internal states
@@ -136,10 +133,6 @@ class TrajectoryControllerBase
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
   // Are we computing the control law or is it valid?
   realtime_tools::RealtimeBuffer<bool> rt_control_law_ready_;
-  // time when the current trajectory started, can be used to interpolate time-varying gains
-  rclcpp::Time trajectory_start_time_;
-  // use this variable to activate new gains from the non-RT thread
-  bool start_trajectory_ = false;
 
   /**
    * @brief compute the control law from the given trajectory

joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp

@@ -78,13 +78,6 @@ bool PidTrajectoryPlugin::updateGainsRT()
   return true;
 }
 
-bool PidTrajectoryPlugin::computeControlLaw(
-  const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/)
-{
-  // nothing to do
-  return true;
-};
-
 void PidTrajectoryPlugin::updateGains()
 {
   for (size_t i = 0; i < num_cmd_joints_; ++i)
@@ -112,8 +105,8 @@ void PidTrajectoryPlugin::updateGains()
 void PidTrajectoryPlugin::computeCommands(
   std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint /*current*/,
   const trajectory_msgs::msg::JointTrajectoryPoint error,
-  const trajectory_msgs::msg::JointTrajectoryPoint desired, const rclcpp::Time & /*time*/,
-  const rclcpp::Duration & period)
+  const trajectory_msgs::msg::JointTrajectoryPoint desired,
+  const rclcpp::Duration & /*duration_since_start*/, const rclcpp::Duration & period)
 {
   // Update PIDs
   for (auto i = 0ul; i < num_cmd_joints_; ++i)

joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp

@@ -45,11 +45,11 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
   traj_msg.positions.push_back(0.0);
   traj_msg.velocities.push_back(0.0);
   std::vector<double> tmp_command(1, 0.0);
-  const rclcpp::Time time;
+  const rclcpp::Duration time_since_start(1, 0);
   const rclcpp::Duration period(1, 0);
 
-  ASSERT_NO_THROW(
-    traj_contr->computeCommands(tmp_command, traj_msg, traj_msg, traj_msg, time, period));
+  ASSERT_NO_THROW(traj_contr->computeCommands(
+    tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
 }
 
 TEST_F(PidTrajectoryTest, TestMultipleJoints)
@@ -86,11 +86,11 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
   traj_msg.velocities.push_back(0.0);
   traj_msg.velocities.push_back(0.0);
   std::vector<double> tmp_command(3, 0.0);
-  const rclcpp::Time time;
+  const rclcpp::Duration time_since_start(1, 0);
   const rclcpp::Duration period(1, 0);
 
-  ASSERT_NO_THROW(
-    traj_contr->computeCommands(tmp_command, traj_msg, traj_msg, traj_msg, time, period));
+  ASSERT_NO_THROW(traj_contr->computeCommands(
+    tmp_command, traj_msg, traj_msg, traj_msg, time_since_start, period));
 
   EXPECT_EQ(tmp_command[0], 1.0);
   EXPECT_EQ(tmp_command[1], 2.0);