Add RT buffer to base class

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -127,26 +127,19 @@ controller_interface::return_type JointTrajectoryController::update(
   {
     params_ = param_listener_->get_params();
     default_tolerances_ = get_segment_tolerances(params_);
-    // update the PID gains
-    // variable use_closed_loop_pid_adapter_ is updated in on_configure only
-    if (use_closed_loop_pid_adapter_)
+
+    // update gains of controller
+    // variable use_closed_loop_control_law_ is updated in on_configure only
+    if (use_closed_loop_control_law_)
     {
-      update_pids();
+      if (traj_contr_->updateGainsRT() == false)
+      {
+        RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller");
+        return controller_interface::return_type::ERROR;
+      }
     }
   }
 
-  // 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_->updateGainsRT() == false)
-  //   {
-  //     RCLCPP_ERROR(get_node()->get_logger(), "Could not update gains of controller");
-  //     return controller_interface::return_type::ERROR;
-  //   }
-  // }
-
   auto compute_error_for_joint = [&](
                                    JointTrajectoryPoint & error, size_t index,
                                    const JointTrajectoryPoint & current,
@@ -182,11 +175,13 @@ controller_interface::return_type JointTrajectoryController::update(
   // Check if a new external message has been received from nonRT threads
   auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg();
   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_)
-  // TODO(christophfroehlich) wait until gains were computed by the trajectory controller
+  // 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_
   if (
     current_external_msg != *new_external_msg &&
-    (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false)
+    (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false &&
+    (use_closed_loop_control_law_ == false || traj_contr_->is_valid()))
   {
     fill_partial_goal(*new_external_msg);
     sort_to_local_joint_order(*new_external_msg);
@@ -768,7 +763,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
         params_.controller_plugin.c_str(), ex.what());
       return CallbackReturn::FAILURE;
     }
-    if (traj_contr_->initialize(get_node(), command_joint_names_) == false)
+    if (traj_contr_->initialize(get_node()) == false)
     {
       RCLCPP_FATAL(
         logger,
@@ -778,9 +773,20 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     }
     else
     {
-      RCLCPP_INFO(
-        logger, "The trajectory controller plugin `%s` was loaded.",
-        params_.controller_plugin.c_str());
+      if (traj_contr_->configure() == false)
+      {
+        RCLCPP_FATAL(
+          logger,
+          "The trajectory controller plugin `%s` failed to initialize for some reason. Aborting.",
+          params_.controller_plugin.c_str());
+        return CallbackReturn::FAILURE;
+      }
+      else
+      {
+        RCLCPP_INFO(
+          logger, "The trajectory controller plugin `%s` was loaded and configured.",
+          params_.controller_plugin.c_str());
+      }
     }
 
     tmp_command_.resize(dof_, 0.0);
@@ -1037,6 +1043,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
     cmd_timeout_ = 0.0;
   }
 
+  if (use_closed_loop_control_law_)
+  {
+    traj_contr_->activate();
+  }
+
   return CallbackReturn::SUCCESS;
 }
 
@@ -1507,7 +1518,7 @@ void JointTrajectoryController::add_new_trajectory_msg(
   // compute gains of controller
   if (use_closed_loop_control_law_)
   {
-    if (traj_contr_->computeGainsNonRT(traj_msg) == false)
+    if (traj_contr_->computeControlLawNonRT(traj_msg) == false)
     {
       RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
     }

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp

@@ -33,11 +33,13 @@ namespace joint_trajectory_controller_plugins
 class PidTrajectoryPlugin : public TrajectoryControllerBase
 {
 public:
-  bool initialize(
-    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
-    std::vector<std::string> command_joint_names) override;
+  bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) override;
 
-  bool computeGainsNonRT(
+  bool configure() override;
+
+  bool activate() override;
+
+  bool computeControlLaw(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override;
 
   bool updateGainsRT() override;
@@ -58,8 +60,6 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
 
   // number of command joints
   size_t num_cmd_joints_;
-  // name of the command joints
-  std::vector<std::string> command_joint_names_;
   // PID controllers
   std::vector<PidPtr> pids_;
   // Feed-forward velocity weight factor when calculating closed loop pid adapter's command

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp

@@ -22,6 +22,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "realtime_tools/realtime_buffer.h"
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 
@@ -40,26 +41,50 @@ class TrajectoryControllerBase
 
   /**
    * @brief initialize the controller plugin.
-   * parse read-only parameters and do pre-allocate memory for the controller
+   * declare parameters
    */
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
-  virtual bool initialize(
-    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
-    std::vector<std::string> command_joint_names) = 0;
+  virtual bool initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node) = 0;
 
   /**
-   * @brief compute the gains from the given trajectory from a non-RT thread
+   * @brief configure the controller plugin.
+   * parse read-only parameters, pre-allocate memory for the controller
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+  virtual bool configure() = 0;
+
+  /**
+   * @brief activate the controller plugin.
+   * parse parameters
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+  virtual bool activate() = 0;
+
+  /**
+   * @brief compute the control law for the given trajectory
+   *
+   * this method can take more time to compute the control law. Hence, it will block the execution
+   * of the trajectory until it finishes
+   *
+   * this method is not virtual, any overrides won't be called by JTC. Instead, override
+   * computeControlLaw for your implementation
+   *
    * @return true if the gains were computed, false otherwise
    */
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
-  virtual bool computeGainsNonRT(
-    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
+  bool computeControlLawNonRT(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
+  {
+    rt_controller_valid_.writeFromNonRT(false);
+    auto ret = computeControlLaw(trajectory);
+    rt_controller_valid_.writeFromNonRT(true);
+    return ret;
+  }
 
   /**
-   * @brief update the gains from the given trajectory from a RT thread
+   * @brief update the gains from a RT thread
    *
-   * this method must finish quickly (within one controller-update rate,
-   * and should not allocate memory
+   * this method must finish quickly (within one controller-update rate)
    *
    * @return true if the gains were updated, false otherwise
    */
@@ -82,9 +107,25 @@ class TrajectoryControllerBase
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
   virtual void reset() = 0;
 
+  /**
+   * @return true if the control law is valid (updated with the trajectory)
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+  bool is_valid() { return rt_controller_valid_.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_;
+
+  /**
+   * @brief compute the control law from the given trajectory
+   * @return true if the gains were computed, false otherwise
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+  virtual bool computeControlLaw(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
 };
 
 }  // namespace joint_trajectory_controller_plugins

joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp

@@ -17,26 +17,38 @@
 namespace joint_trajectory_controller_plugins
 {
 
-bool PidTrajectoryPlugin::initialize(
-  rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<std::string> command_joint_names)
+bool PidTrajectoryPlugin::initialize(rclcpp_lifecycle::LifecycleNode::SharedPtr node)
 {
   node_ = node;
-  command_joint_names_ = command_joint_names;
 
   try
   {
     // Create the parameter listener and get the parameters
     param_listener_ = std::make_shared<ParamListener>(node_);
-    params_ = param_listener_->get_params();
   }
   catch (const std::exception & e)
   {
     fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
     return false;
   }
 
+  return true;
+}
+
+bool PidTrajectoryPlugin::configure()
+{
+  try
+  {
+    params_ = param_listener_->get_params();
+  }
+  catch (const std::exception & e)
+  {
+    fprintf(stderr, "Exception thrown during configure stage with message: %s \n", e.what());
+    return false;
+  }
+
   // parse read-only params
-  num_cmd_joints_ = command_joint_names_.size();
+  num_cmd_joints_ = params_.command_joints.size();
   if (num_cmd_joints_ == 0)
   {
     RCLCPP_ERROR(node_->get_logger(), "[PidTrajectoryPlugin] No command joints specified.");
@@ -46,7 +58,14 @@ bool PidTrajectoryPlugin::initialize(
   ff_velocity_scale_.resize(num_cmd_joints_);
 
   return true;
-}
+};
+
+bool PidTrajectoryPlugin::activate()
+{
+  params_ = param_listener_->get_params();
+  updateGains();
+  return true;
+};
 
 bool PidTrajectoryPlugin::updateGainsRT()
 {
@@ -59,11 +78,10 @@ bool PidTrajectoryPlugin::updateGainsRT()
   return true;
 }
 
-bool PidTrajectoryPlugin::computeGainsNonRT(
+bool PidTrajectoryPlugin::computeControlLaw(
   const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/)
 {
-  params_ = param_listener_->get_params();
-  updateGains();
+  // nothing to do
   return true;
 };
 
@@ -72,10 +90,10 @@ void PidTrajectoryPlugin::updateGains()
   for (size_t i = 0; i < num_cmd_joints_; ++i)
   {
     RCLCPP_DEBUG(
-      node_->get_logger(), "[PidTrajectoryPlugin] command_joint_names_ %lu : %s", i,
-      command_joint_names_[i].c_str());
+      node_->get_logger(), "[PidTrajectoryPlugin] params_.command_joints %lu : %s", i,
+      params_.command_joints[i].c_str());
 
-    const auto & gains = params_.gains.command_joints_map.at(command_joint_names_[i]);
+    const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]);
     pids_[i] = std::make_shared<control_toolbox::Pid>(
       gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
     ff_velocity_scale_[i] = gains.ff_velocity_scale;

joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp

@@ -19,7 +19,7 @@ TEST_F(PidTrajectoryTest, TestEmptySetup)
   std::shared_ptr<TestableJointTrajectoryControllerPlugin> traj_contr =
     std::make_shared<TestableJointTrajectoryControllerPlugin>();
 
-  ASSERT_FALSE(traj_contr->initialize(node_, std::vector<std::string>()));
+  ASSERT_FALSE(traj_contr->initialize(node_));
 }
 
 TEST_F(PidTrajectoryTest, TestSingleJoint)
@@ -30,17 +30,15 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
   std::vector<std::string> joint_names = {"joint1"};
   auto joint_names_paramv = rclcpp::ParameterValue(joint_names);
 
-  // override read_only parameter
+  // override parameter
   node_->declare_parameter("command_joints", joint_names_paramv);
 
-  ASSERT_TRUE(traj_contr->initialize(node_, joint_names));
-
-  // set dynamic parameters
-  traj_contr->trigger_declare_parameters();
+  ASSERT_TRUE(traj_contr->initialize(node_));
   node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
-
+  ASSERT_TRUE(traj_contr->configure());
+  ASSERT_TRUE(traj_contr->activate());
   ASSERT_TRUE(
-    traj_contr->computeGainsNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+    traj_contr->computeControlLawNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
   ASSERT_TRUE(traj_contr->updateGainsRT());
 
   trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
@@ -62,19 +60,22 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
   std::vector<std::string> joint_names = {"joint1", "joint2", "joint3"};
   auto joint_names_paramv = rclcpp::ParameterValue(joint_names);
 
-  // override read_only parameter
+  // override parameter
   node_->declare_parameter("command_joints", joint_names_paramv);
 
-  ASSERT_TRUE(traj_contr->initialize(node_, joint_names));
-
+  ASSERT_TRUE(traj_contr->initialize(node_));
   // set dynamic parameters
-  traj_contr->trigger_declare_parameters();
   node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
   node_->set_parameter(rclcpp::Parameter("gains.joint2.p", 2.0));
   node_->set_parameter(rclcpp::Parameter("gains.joint3.p", 3.0));
+  ASSERT_TRUE(traj_contr->configure());
+  ASSERT_TRUE(traj_contr->activate());
+  ASSERT_TRUE(
+    traj_contr->computeControlLawNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+  ASSERT_TRUE(traj_contr->updateGainsRT());
 
   ASSERT_TRUE(
-    traj_contr->computeGainsNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+    traj_contr->computeControlLawNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
   ASSERT_TRUE(traj_contr->updateGainsRT());
 
   trajectory_msgs::msg::JointTrajectoryPoint traj_msg;