Add trajectory start time

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -137,7 +137,7 @@ controller_interface::return_type JointTrajectoryController::update(
   // update gains of controller
   // TODO(christophfroehlich) activate this
   // once https://github.com/ros-controls/ros2_controllers/pull/761 is merged
-  // if (use_closed_loop_control_law_)
+  // if (traj_contr_)
   // {
   //   if(traj_contr_->updateGainsRT() == false)
   //   {
@@ -183,11 +183,11 @@ controller_interface::return_type JointTrajectoryController::update(
   auto new_external_msg = traj_msg_external_point_ptr_.readFromRT();
   // Discard,
   //  if a goal is pending but still not active (somewhere stuck in goal_handle_timer_)
-  //  and if use_closed_loop_control_law_: wait until control law is computed by the traj_contr_
+  //  and if traj_contr_: wait until control law is computed by the traj_contr_
   if (
     current_external_msg != *new_external_msg &&
     (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false &&
-    (use_closed_loop_control_law_ == false || traj_contr_->is_valid()))
+    (traj_contr_ == nullptr || traj_contr_->is_ready()))
   {
     fill_partial_goal(*new_external_msg);
     sort_to_local_joint_order(*new_external_msg);
@@ -226,6 +226,11 @@ controller_interface::return_type JointTrajectoryController::update(
       {
         traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_);
       }
+      if (traj_contr_)
+      {
+        // set start time of trajectory to traj_contr_
+        traj_contr_->start(time, traj_external_point_ptr_->time_from_start());
+      }
     }
 
     // find segment for current timestamp
@@ -298,7 +303,7 @@ controller_interface::return_type JointTrajectoryController::update(
       // set values for next hardware write() if tolerance is met
       if (!tolerance_violated_while_moving && within_goal_time)
       {
-        if (use_closed_loop_control_law_)
+        if (traj_contr_)
         {
           traj_contr_->computeCommands(
             tmp_command_, state_current_, state_error_, state_desired_, time, period);
@@ -1038,7 +1043,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
     read_state_from_state_interfaces(last_commanded_state_);
   }
 
-  if (use_closed_loop_control_law_)
+  if (traj_contr_)
   {
     traj_contr_->activate();
   }
@@ -1552,7 +1557,7 @@ void JointTrajectoryController::add_new_trajectory_msg(
   traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);
 
   // compute gains of controller
-  if (use_closed_loop_control_law_)
+  if (traj_contr_)
   {
     if (traj_contr_->computeControlLawNonRT(traj_msg) == false)
     {

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp

@@ -75,12 +75,30 @@ class TrajectoryControllerBase
   bool computeControlLawNonRT(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
   {
-    rt_controller_valid_.writeFromNonRT(false);
+    rt_control_law_ready_.writeFromNonRT(false);
     auto ret = computeControlLaw(trajectory);
-    rt_controller_valid_.writeFromNonRT(true);
+    rt_control_law_ready_.writeFromNonRT(true);
     return ret;
   }
 
+  /**
+   * @brief set the time when the current trajectory started
+   *
+   * (same logic as trajectory class)
+   */
+  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;
+  }
+
   /**
    * @brief update the gains from a RT thread
    *
@@ -108,16 +126,20 @@ class TrajectoryControllerBase
   virtual void reset() = 0;
 
   /**
-   * @return true if the control law is valid (updated with the trajectory)
+   * @return true if the control law is ready (updated with the trajectory)
    */
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
-  bool is_valid() { return rt_controller_valid_.readFromRT(); }
+  bool is_ready() { return rt_control_law_ready_.readFromRT(); }
 
 protected:
   // the node handle for parameter handling
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
   // Are we computing the control law or is it valid?
-  realtime_tools::RealtimeBuffer<bool> rt_controller_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