Rework plugin API

Author: christophfroehlich

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/pid_trajectory_plugin.hpp

@@ -32,60 +32,64 @@ namespace joint_trajectory_controller_plugins
 class PidTrajectoryPlugin : public TrajectoryControllerBase
 {
 public:
-  bool initialize(
-    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
-    std::vector<size_t> map_cmd_to_joints) override;
+  bool update_gains_rt() override;
 
-  bool configure() override;
+  void compute_commands(
+    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::Duration & duration_since_start, const rclcpp::Duration & period) override;
 
-  bool activate() override;
+  void reset() override;
 
-  bool compute_control_law_non_rt_impl(
-    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+  void start() override
   {
     // nothing to do
-    return true;
   }
 
-  bool compute_control_law_rt_impl(
-    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+protected:
+  // Parameters from ROS for joint_trajectory_controller_plugins
+  std::shared_ptr<ParamListener> param_listener_;
+
+  bool on_initialize(void) override;
+
+  rclcpp::Logger set_logger() const override
   {
-    // nothing to do
-    return true;
+    return node_->get_logger().get_child("PidTrajectoryPlugin");
   }
 
-  bool update_gains_rt() override;
+  bool on_configure() override;
 
-  void compute_commands(
-    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::Duration & duration_since_start, const rclcpp::Duration & period) override;
+  bool on_activate() override;
 
-  void reset() override;
+  bool on_compute_control_law_non_rt(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
+  {
+    // nothing to do
+    return true;
+  }
 
-  void start() override
+  bool on_compute_control_law_rt(
+    const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & /*trajectory*/) override
   {
     // nothing to do
+    return true;
   }
 
-protected:
+private:
   /**
    * @brief parse PID gains from parameter struct
    */
   void parse_gains();
 
   // number of command joints
   size_t num_cmd_joints_;
-  // map from joints in the message to command joints
-  std::vector<size_t> map_cmd_to_joints_;
   // PID controllers
   std::vector<PidPtr> pids_;
   // Feed-forward velocity weight factor when calculating closed loop pid adapter's command
   std::vector<double> ff_velocity_scale_;
 
   // Parameters from ROS for joint_trajectory_controller_plugins
-  std::shared_ptr<ParamListener> param_listener_;
   Params params_;
 };
 

joint_trajectory_controller_plugins/include/joint_trajectory_controller_plugins/trajectory_controller_base.hpp

@@ -37,25 +37,28 @@ class TrajectoryControllerBase
 
   /**
    * @brief initialize the controller plugin.
-   * declare parameters
    * @param node the node handle to use for parameter handling
    * @param map_cmd_to_joints a mapping from the joint names in the trajectory messages to the
    * command joints
    */
-  virtual bool initialize(
-    rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints) = 0;
+  bool initialize(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints)
+  {
+    node_ = node;
+    map_cmd_to_joints_ = map_cmd_to_joints;
+    logger_ = this->set_logger();
+    return on_initialize();
+  };
 
   /**
    * @brief configure the controller plugin.
-   * parse read-only parameters, pre-allocate memory for the controller
    */
-  virtual bool configure() = 0;
+  bool configure() { return on_configure(); }
 
   /**
    * @brief activate the controller plugin.
-   * parse parameters
    */
-  virtual bool activate() = 0;
+  bool activate() { return on_activate(); }
 
   /**
    * @brief compute the control law for the given trajectory
@@ -64,15 +67,15 @@ class TrajectoryControllerBase
    * of the trajectory until it finishes
    *
    * this method is not virtual, any overrides won't be called by JTC. Instead, override
-   * compute_control_law_non_rt_impl for your implementation
+   * on_compute_control_law_non_rt for your implementation
    *
    * @return true if the gains were computed, false otherwise
    */
   bool compute_control_law_non_rt(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory)
   {
     rt_control_law_ready_.writeFromNonRT(false);
-    auto ret = compute_control_law_non_rt_impl(trajectory);
+    auto ret = on_compute_control_law_non_rt(trajectory);
     rt_control_law_ready_.writeFromNonRT(true);
     return ret;
   }
@@ -83,7 +86,7 @@ class TrajectoryControllerBase
    * this method must finish quickly (within one controller-update rate)
    *
    * this method is not virtual, any overrides won't be called by JTC. Instead, override
-   * compute_control_law_rt_impl for your implementation
+   * on_compute_control_law_rt for your implementation
    *
    * @return true if the gains were computed, false otherwise
    */
@@ -92,7 +95,7 @@ class TrajectoryControllerBase
   {
     // TODO(christophfroehlich): Need a lock-free write here
     rt_control_law_ready_.writeFromNonRT(false);
-    auto ret = compute_control_law_rt_impl(trajectory);
+    auto ret = on_compute_control_law_rt(trajectory);
     rt_control_law_ready_.writeFromNonRT(true);
     return ret;
   }
@@ -145,22 +148,58 @@ class TrajectoryControllerBase
 protected:
   // the node handle for parameter handling
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+  // map from joints in the message to command joints
+  std::vector<size_t> map_cmd_to_joints_;
   // Are we computing the control law or is it valid?
   realtime_tools::RealtimeBuffer<bool> rt_control_law_ready_;
 
+  /**
+   * @brief Get the logger for this plugin
+   */
+  rclcpp::Logger get_logger() const { return logger_; }
+  /**
+   * @brief Get the logger for this plugin
+   */
+  virtual rclcpp::Logger set_logger() const = 0;
+
   /**
    * @brief compute the control law from the given trajectory (in the non-RT loop)
    * @return true if the gains were computed, false otherwise
    */
-  virtual bool compute_control_law_non_rt_impl(
+  virtual bool on_compute_control_law_non_rt(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
 
   /**
    * @brief compute the control law for a single point (in the RT loop)
    * @return true if the gains were computed, false otherwise
    */
-  virtual bool compute_control_law_rt_impl(
+  virtual bool on_compute_control_law_rt(
     const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> & trajectory) = 0;
+
+  /**
+   * @brief initialize the controller plugin.
+   *
+   * declare parameters
+   */
+  virtual bool on_initialize(void) = 0;
+
+  /**
+   * @brief configure the controller plugin.
+   *
+   * parse read-only parameters, pre-allocate memory for the controller
+   */
+  virtual bool on_configure() = 0;
+
+  /**
+   * @brief activate the controller plugin.
+   *
+   * parse parameters
+   */
+  virtual bool on_activate() = 0;
+
+private:
+  // child logger for this plugin
+  rclcpp::Logger logger_ = rclcpp::get_logger("joint_trajectory_controller_plugins");
 };
 
 }  // namespace joint_trajectory_controller_plugins

joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp

@@ -17,12 +17,8 @@
 namespace joint_trajectory_controller_plugins
 {
 
-bool PidTrajectoryPlugin::initialize(
-  rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::vector<size_t> map_cmd_to_joints)
+bool PidTrajectoryPlugin::on_initialize()
 {
-  node_ = node;
-  map_cmd_to_joints_ = map_cmd_to_joints;
-
   try
   {
     // Create the parameter listener and get the parameters
@@ -37,7 +33,7 @@ bool PidTrajectoryPlugin::initialize(
   return true;
 }
 
-bool PidTrajectoryPlugin::configure()
+bool PidTrajectoryPlugin::on_configure()
 {
   try
   {
@@ -53,14 +49,12 @@ bool PidTrajectoryPlugin::configure()
   num_cmd_joints_ = params_.command_joints.size();
   if (num_cmd_joints_ == 0)
   {
-    RCLCPP_ERROR(node_->get_logger(), "[PidTrajectoryPlugin] No command joints specified.");
+    RCLCPP_ERROR(get_logger(), "No command joints specified.");
     return false;
   }
   if (num_cmd_joints_ != map_cmd_to_joints_.size())
   {
-    RCLCPP_ERROR(
-      node_->get_logger(),
-      "[PidTrajectoryPlugin] map_cmd_to_joints has to be of size num_cmd_joints.");
+    RCLCPP_ERROR(get_logger(), "map_cmd_to_joints has to be of size num_cmd_joints.");
     return false;
   }
   pids_.resize(num_cmd_joints_);  // memory for the shared pointers, will be nullptr
@@ -74,7 +68,7 @@ bool PidTrajectoryPlugin::configure()
   return true;
 }
 
-bool PidTrajectoryPlugin::activate()
+bool PidTrajectoryPlugin::on_activate()
 {
   params_ = param_listener_->get_params();
   parse_gains();
@@ -97,8 +91,7 @@ void PidTrajectoryPlugin::parse_gains()
   for (size_t i = 0; i < num_cmd_joints_; ++i)
   {
     RCLCPP_DEBUG(
-      node_->get_logger(), "[PidTrajectoryPlugin] params_.command_joints %lu : %s", i,
-      params_.command_joints[i].c_str());
+      get_logger(), "params_.command_joints %lu : %s", i, params_.command_joints[i].c_str());
 
     const auto & gains = params_.gains.command_joints_map.at(params_.command_joints[i]);
     control_toolbox::AntiWindupStrategy antiwindup_strat;
@@ -111,15 +104,12 @@ void PidTrajectoryPlugin::parse_gains()
       gains.p, gains.i, gains.d, gains.u_clamp_max, gains.u_clamp_min, antiwindup_strat);
     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_DEBUG(get_logger(), "gains.p: %f", gains.p);
+    RCLCPP_DEBUG(get_logger(), "ff_velocity_scale_: %f", ff_velocity_scale_[i]);
   }
 
   RCLCPP_INFO(
-    node_->get_logger(),
-    "[PidTrajectoryPlugin] Loaded PID gains from ROS parameters for %lu joint(s).",
-    num_cmd_joints_);
+    get_logger(), "Loaded PID gains from ROS parameters for %lu joint(s).", num_cmd_joints_);
 }
 
 void PidTrajectoryPlugin::compute_commands(