Split non-RT and RT methods

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -132,6 +132,18 @@ controller_interface::return_type JointTrajectoryController::update(
     return controller_interface::return_type::OK;
   }
 
+  // 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,
@@ -168,6 +180,7 @@ controller_interface::return_type JointTrajectoryController::update(
   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
   if (
     current_external_msg != *new_external_msg &&
     (*(rt_has_pending_goal_.readFromRT()) && !active_goal) == false)
@@ -718,7 +731,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     contains_interface_type(params_.command_interfaces, hardware_interface::HW_IF_EFFORT);
 
   // if there is only velocity or if there is effort command interface
-  // then use also closed loop controller
+  // then use also closed-loop controller
   use_closed_loop_control_law_ =
     (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&
      !params_.open_loop_control) ||
@@ -1502,10 +1515,13 @@ void JointTrajectoryController::add_new_trajectory_msg(
 {
   traj_msg_external_point_ptr_.writeFromNonRT(traj_msg);
 
-  // update gains of controller
+  // compute gains of controller
   if (use_closed_loop_control_law_)
   {
-    traj_contr_->computeGains(traj_msg);
+    if (traj_contr_->computeGainsNonRT(traj_msg) == false)
+    {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
+    }
   }
 }
 

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -215,6 +215,7 @@ class TrajectoryControllerTest : public ::testing::Test
     traj_controller_->trigger_declare_parameters();
     auto node = traj_controller_->get_node();
 
+    // TODO(christophfroehlich): use command joints instead of joint_names
     for (size_t i = 0; i < joint_names_.size(); ++i)
     {
       const std::string prefix = "gains." + joint_names_[i];

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp

@@ -37,8 +37,10 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
     rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::vector<std::string> command_joint_names) override;
 
-  bool computeGains(
-    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) override;
+  bool computeGainsNonRT(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override;
+
+  bool updateGainsRT() override;
 
   void computeCommands(
     std::vector<double> & tmp_command, const trajectory_msgs::msg::JointTrajectoryPoint current,
@@ -49,6 +51,11 @@ class PidTrajectoryPlugin : public TrajectoryControllerBase
   void reset() override;
 
 protected:
+  /**
+   * @brief parse PID gains from parameter struct
+   */
+  void updateGains();
+
   // number of command joints
   size_t num_cmd_joints_;
   // name of the command joints

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp

@@ -39,19 +39,35 @@ class TrajectoryControllerBase
   virtual ~TrajectoryControllerBase() = default;
 
   /**
+   * @brief initialize the controller plugin.
+   * parse read-only parameters and do pre-allocate memory for the controller
    */
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
   virtual bool initialize(
     rclcpp_lifecycle::LifecycleNode::SharedPtr node,
     std::vector<std::string> command_joint_names) = 0;
 
   /**
+   * @brief compute the gains from the given trajectory from a non-RT thread
+   * @return true if the gains were computed, false otherwise
    */
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
-  virtual bool computeGains(
+  virtual bool computeGainsNonRT(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
 
   /**
+   * @brief update the gains from the given trajectory from a RT thread
+   *
+   * this method must finish quickly (within one controller-update rate,
+   * and should not allocate memory
+   *
+   * @return true if the gains were updated, false otherwise
+   */
+  JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
+  virtual bool updateGainsRT(void) = 0;
+
+  /**
+   * @brief compute the commands with the calculated gains
    */
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
   virtual void computeCommands(
@@ -61,6 +77,7 @@ class TrajectoryControllerBase
     const rclcpp::Duration & period) = 0;
 
   /**
+   * @brief reset internal states
    */
   JOINT_TRAJECTORY_CONTROLLER_PLUGINS_PUBLIC
   virtual void reset() = 0;

joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp

@@ -45,37 +45,50 @@ bool PidTrajectoryPlugin::initialize(
   pids_.resize(num_cmd_joints_);
   ff_velocity_scale_.resize(num_cmd_joints_);
 
-  RCLCPP_INFO(
-    node_->get_logger(), "[PidTrajectoryPlugin] Initialized with %lu joints.", num_cmd_joints_);
   return true;
 }
 
-bool PidTrajectoryPlugin::computeGains(
-  const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/)
+bool PidTrajectoryPlugin::updateGainsRT()
 {
   if (param_listener_->is_old(params_))
   {
     params_ = param_listener_->get_params();
-    RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] Updated parameters");
+    updateGains();
   }
 
-  pids_.resize(num_cmd_joints_);
-  ff_velocity_scale_.resize(num_cmd_joints_);
+  return true;
+}
 
-  // Init PID gains from ROS parameters
+bool PidTrajectoryPlugin::computeGainsNonRT(
+  const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/)
+{
+  params_ = param_listener_->get_params();
+  updateGains();
+  return true;
+};
+
+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());
+
     const auto & gains = params_.gains.joints_map.at(command_joint_names_[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;
+
+    RCLCPP_DEBUG(node_->get_logger(), "[PidTrajectoryPlugin] gains.p: %f", gains.p);
+    RCLCPP_DEBUG(
+      node_->get_logger(), "[PidTrajectoryPlugin] ff_velocity_scale_: %f", ff_velocity_scale_[i]);
   }
 
   RCLCPP_INFO(
     node_->get_logger(),
-    "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joints.", num_cmd_joints_);
-  return true;
+    "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joint(s).",
+    num_cmd_joints_);
 }
 
 void PidTrajectoryPlugin::computeCommands(

joint_trajectory_controller_plugins/test/test_pid_trajectory.cpp

@@ -39,7 +39,9 @@ TEST_F(PidTrajectoryTest, TestSingleJoint)
   traj_contr->trigger_declare_parameters();
   node_->set_parameter(rclcpp::Parameter("gains.joint1.p", 1.0));
 
-  ASSERT_TRUE(traj_contr->computeGains(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+  ASSERT_TRUE(
+    traj_contr->computeGainsNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+  ASSERT_TRUE(traj_contr->updateGainsRT());
 
   trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
   traj_msg.positions.push_back(0.0);
@@ -68,15 +70,17 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
   // 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", 1.0));
-  node_->set_parameter(rclcpp::Parameter("gains.joint3.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->computeGains(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+  ASSERT_TRUE(
+    traj_contr->computeGainsNonRT(std::make_shared<trajectory_msgs::msg::JointTrajectory>()));
+  ASSERT_TRUE(traj_contr->updateGainsRT());
 
   trajectory_msgs::msg::JointTrajectoryPoint traj_msg;
-  traj_msg.positions.push_back(0.0);
-  traj_msg.positions.push_back(0.0);
-  traj_msg.positions.push_back(0.0);
+  traj_msg.positions.push_back(1.0);
+  traj_msg.positions.push_back(1.0);
+  traj_msg.positions.push_back(1.0);
   traj_msg.velocities.push_back(0.0);
   traj_msg.velocities.push_back(0.0);
   traj_msg.velocities.push_back(0.0);
@@ -86,6 +90,10 @@ TEST_F(PidTrajectoryTest, TestMultipleJoints)
 
   ASSERT_NO_THROW(
     traj_contr->computeCommands(tmp_command, traj_msg, traj_msg, traj_msg, time, period));
+
+  EXPECT_EQ(tmp_command[0], 1.0);
+  EXPECT_EQ(tmp_command[1], 2.0);
+  EXPECT_EQ(tmp_command[2], 3.0);
 }
 
 int main(int argc, char ** argv)