Add support for getting PID parameters from loaded parameters

doc/index.rst

@@ -185,6 +185,39 @@ Where the parameters are as follows:
 
 The same definitions apply to the ``vel_*`` parameters.
 
+The PID parameters can be defined for ``position`` or ``position_pid`` and ``velocity`` or ``velocity_pid`` command interfaces as explained above, or definiting them in a yaml file and loading it in the ``gazebo_ros2_control`` plugin as below:
+
+.. code-block:: yaml
+
+  gazebo_ros2_control:
+  ros__parameters:
+    pid_gains:
+      position:
+        slider_to_cart: {kp:  100.0, kd: 10.0, ki:  1.0, max_integral_error: 10000.0}
+
+
+.. code-block:: yaml
+
+  gazebo_ros2_control:
+  ros__parameters:
+    pid_gains:
+      position_pid:
+        slider_to_cart: {kp:  100.0, kd: 10.0, ki:  1.0, max_integral_error: 10000.0}
+
+.. code-block:: xml
+
+  <gazebo>
+    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      ...
+      <ros>
+        <argument>--ros-args</argument>
+        <argument>--params-file</argument>
+        <argument>Path to the configuration file</argument>
+      </ros>
+    </plugin>
+  </gazebo>
+
+
 Add the gazebo_ros2_control plugin
 ==========================================
 

gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp

@@ -90,9 +90,12 @@ class GazeboSystem : public GazeboSystemInterface
     const hardware_interface::HardwareInfo & hardware_info,
     gazebo::physics::ModelPtr parent_model);
 
-  control_toolbox::Pid extractPID(
-    std::string prefix,
-    hardware_interface::ComponentInfo joint_info);
+  bool extractPID(
+    const std::string & prefix,
+    const hardware_interface::ComponentInfo & joint_info, control_toolbox::Pid & pid);
+
+  bool extractPIDFromParameters(
+    const std::string & control_mode, const std::string & joint_name, control_toolbox::Pid & pid);
 
   /// \brief Private data class
   std::unique_ptr<GazeboSystemPrivate> dataPtr;

gazebo_ros2_control/src/gazebo_system.cpp

@@ -12,6 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <cmath>
 #include <limits>
 #include <map>
 #include <memory>
@@ -110,6 +111,12 @@ class gazebo_ros2_control::GazeboSystemPrivate
 
   // Should hold the joints if no control_mode is active
   bool hold_joints_ = true;
+
+  // Current position and velocity control method
+  GazeboSystemInterface::ControlMethod_ position_control_method_ =
+    GazeboSystemInterface::ControlMethod_::POSITION;
+  GazeboSystemInterface::ControlMethod_ velocity_control_method_ =
+    GazeboSystemInterface::ControlMethod_::VELOCITY;
 };
 
 namespace gazebo_ros2_control
@@ -157,7 +164,8 @@ bool GazeboSystem::initSim(
       this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
   }
   RCLCPP_DEBUG_STREAM(
-    this->nh_->get_logger(), "hold_joints (system): " << this->dataPtr->hold_joints_ << std::endl);
+    this->nh_->get_logger(),
+    "hold_joints (system): " << std::boolalpha << this->dataPtr->hold_joints_ << std::endl);
 
   registerJoints(hardware_info, parent_model);
   registerSensors(hardware_info, parent_model);
@@ -170,9 +178,9 @@ bool GazeboSystem::initSim(
   return true;
 }
 
-control_toolbox::Pid GazeboSystem::extractPID(
-  std::string prefix,
-  hardware_interface::ComponentInfo joint_info)
+bool GazeboSystem::extractPID(
+  const std::string & prefix,
+  const hardware_interface::ComponentInfo & joint_info, control_toolbox::Pid & pid)
 {
   double kp;
   double ki;
@@ -181,52 +189,127 @@ control_toolbox::Pid GazeboSystem::extractPID(
   double min_integral_error;
   bool antiwindup = false;
 
+  bool are_pids_set = false;
   if (joint_info.parameters.find(prefix + "kp") != joint_info.parameters.end()) {
     kp = std::stod(joint_info.parameters.at(prefix + "kp"));
+    are_pids_set = true;
   } else {
     kp = 0.0;
   }
 
   if (joint_info.parameters.find(prefix + "ki") != joint_info.parameters.end()) {
     ki = std::stod(joint_info.parameters.at(prefix + "ki"));
+    are_pids_set = true;
   } else {
     ki = 0.0;
   }
 
   if (joint_info.parameters.find(prefix + "kd") != joint_info.parameters.end()) {
     kd = std::stod(joint_info.parameters.at(prefix + "kd"));
+    are_pids_set = true;
   } else {
     kd = 0.0;
   }
 
-  if (joint_info.parameters.find(prefix + "max_integral_error") != joint_info.parameters.end()) {
-    max_integral_error = std::stod(joint_info.parameters.at(prefix + "max_integral_error"));
-  } else {
-    max_integral_error = std::numeric_limits<double>::max();
-  }
+  if (are_pids_set) {
+    if (joint_info.parameters.find(prefix + "max_integral_error") != joint_info.parameters.end()) {
+      max_integral_error = std::stod(joint_info.parameters.at(prefix + "max_integral_error"));
+    } else {
+      max_integral_error = std::numeric_limits<double>::max();
+    }
 
-  if (joint_info.parameters.find(prefix + "min_integral_error") != joint_info.parameters.end()) {
-    min_integral_error = std::stod(joint_info.parameters.at(prefix + "min_integral_error"));
-  } else {
-    min_integral_error = std::numeric_limits<double>::min();
-  }
+    if (joint_info.parameters.find(prefix + "min_integral_error") != joint_info.parameters.end()) {
+      min_integral_error = std::stod(joint_info.parameters.at(prefix + "min_integral_error"));
+    } else {
+      min_integral_error = std::numeric_limits<double>::min();
+    }
 
-  if (joint_info.parameters.find(prefix + "antiwindup") != joint_info.parameters.end()) {
-    if (joint_info.parameters.at(prefix + "antiwindup") == "true" ||
-      joint_info.parameters.at(prefix + "antiwindup") == "True")
-    {
-      antiwindup = true;
+    if (joint_info.parameters.find(prefix + "antiwindup") != joint_info.parameters.end()) {
+      if (joint_info.parameters.at(prefix + "antiwindup") == "true" ||
+        joint_info.parameters.at(prefix + "antiwindup") == "True")
+      {
+        antiwindup = true;
+      }
     }
+
+    RCLCPP_INFO_STREAM(
+      this->nh_->get_logger(),
+      "Setting kp = " << kp << "\t"
+                      << " ki = " << ki << "\t"
+                      << " kd = " << kd << "\t"
+                      << " max_integral_error = " << max_integral_error << "\t"
+                      << "antiwindup =" << std::boolalpha << antiwindup);
+
+    pid.initPid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup);
   }
+  return are_pids_set;
+}
 
-  RCLCPP_INFO_STREAM(
-    this->nh_->get_logger(),
-    "Setting kp = " << kp << "\t"
-                    << " ki = " << ki << "\t"
-                    << " kd = " << kd << "\t"
-                    << " max_integral_error = " << max_integral_error);
 
-  return control_toolbox::Pid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup);
+bool GazeboSystem::extractPIDFromParameters(
+  const std::string & control_mode, const std::string & joint_name,
+  control_toolbox::Pid & pid)
+{
+  const std::string parameter_prefix = "pid_gains." + control_mode + "." + joint_name;
+  auto get_pid_entry = [this, parameter_prefix](const std::string & entry, double & value) -> bool {
+      try {
+        // Check if the parameter is declared, if not, declare the default value NaN
+        if (!this->nh_->has_parameter(parameter_prefix + "." + entry)) {
+          this->nh_->declare_parameter<double>(
+            parameter_prefix + "." + entry,
+            std::numeric_limits<double>::quiet_NaN());
+        }
+        value = this->nh_->get_parameter(parameter_prefix + "." + entry).as_double();
+      } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+        RCLCPP_ERROR(
+          this->nh_->get_logger(),
+          "Parameter '%s' not declared, with error %s", entry.c_str(), ex.what());
+        return false;
+      } catch (rclcpp::exceptions::InvalidParameterTypeException & ex) {
+        RCLCPP_ERROR(
+          this->nh_->get_logger(),
+          "Parameter '%s' has wrong type: %s", entry.c_str(), ex.what());
+        return false;
+      }
+      return std::isfinite(value);
+    };
+  bool are_pids_set = true;
+  double kp, ki, kd, max_integral_error, min_integral_error;
+  bool antiwindup;
+  are_pids_set &= get_pid_entry("kp", kp);
+  are_pids_set &= get_pid_entry("ki", ki);
+  are_pids_set &= get_pid_entry("kd", kd);
+  if (are_pids_set) {
+    get_pid_entry("max_integral_error", max_integral_error);
+    get_pid_entry("min_integral_error", min_integral_error);
+    const std::string antiwindup_str = "antiwindup";
+    if (!this->nh_->has_parameter(parameter_prefix + "." + antiwindup_str)) {
+      this->nh_->declare_parameter(
+        parameter_prefix + "." + antiwindup_str,
+        false);
+    }
+    try {
+      antiwindup = this->nh_->get_parameter(parameter_prefix + "." + antiwindup_str).as_bool();
+    } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+      RCLCPP_ERROR(
+        this->nh_->get_logger(),
+        "Parameter '%s' not declared, with error %s", antiwindup_str.c_str(), ex.what());
+    } catch (rclcpp::ParameterTypeException & ex) {
+      RCLCPP_ERROR(
+        this->nh_->get_logger(),
+        "Parameter '%s' has wrong type: %s", antiwindup_str.c_str(), ex.what());
+    }
+    RCLCPP_INFO_STREAM(
+      this->nh_->get_logger(),
+      "Setting kp = " << kp << "\t"
+                      << " ki = " << ki << "\t"
+                      << " kd = " << kd << "\t"
+                      << " max_integral_error = " << max_integral_error << "\t"
+                      << "antiwindup =" << std::boolalpha << antiwindup << " from node parameters");
+    pid.initPid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup);
+  }
+
+  return are_pids_set;
 }
 
 void GazeboSystem::registerJoints(
@@ -251,7 +334,7 @@ void GazeboSystem::registerJoints(
 
   for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
     auto & joint_info = hardware_info.joints[j];
-    std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name;
+    const std::string joint_name = this->dataPtr->joint_names_[j] = joint_info.name;
 
     gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name);
     this->dataPtr->sim_joints_.push_back(simjoint);
@@ -352,20 +435,24 @@ void GazeboSystem::registerJoints(
         if (has_already_registered_pos) {
           RCLCPP_ERROR_STREAM(
             this->nh_->get_logger(),
-            "can't have position and position"
-              << "pid command_interface at same time !!!");
+            "can't have position and position_pid"
+              << "command_interface at same time !!!");
           continue;
         }
         has_already_registered_pos = true;
         RCLCPP_INFO_STREAM(
           this->nh_->get_logger(), "\t\t "
             << joint_info.command_interfaces[i].name);
 
-        if (joint_info.command_interfaces[i].name == "position_pid") {
-          this->dataPtr->is_pos_pid[j] = true;
-          this->dataPtr->pos_pid[j] = this->extractPID(POSITION_PID_PARAMS_PREFIX, joint_info);
-        } else {
-          this->dataPtr->is_pos_pid[j] = false;
+        this->dataPtr->is_pos_pid[j] = this->extractPID(
+          POSITION_PID_PARAMS_PREFIX, joint_info,
+          this->dataPtr->pos_pid[j]) || this->extractPIDFromParameters(
+          joint_info.command_interfaces[i].name, joint_name, this->dataPtr->pos_pid[j]);
+
+        if (this->dataPtr->is_pos_pid[j]) {
+          RCLCPP_INFO_STREAM(
+            this->nh_->get_logger(), "Found position PIDs for joint: " << joint_name << "!");
+          this->dataPtr->position_control_method_ = POSITION_PID;
         }
 
         this->dataPtr->command_interfaces_.emplace_back(
@@ -395,10 +482,17 @@ void GazeboSystem::registerJoints(
           this->nh_->get_logger(), "\t\t "
             << joint_info.command_interfaces[i].name);
 
-        if (joint_info.command_interfaces[i].name == "velocity_pid") {
-          this->dataPtr->is_vel_pid[j] = true;
-          this->dataPtr->vel_pid[j] = this->extractPID(VELOCITY_PID_PARAMS_PREFIX, joint_info);
+        this->dataPtr->is_vel_pid[j] = this->extractPID(
+          VELOCITY_PID_PARAMS_PREFIX, joint_info,
+          this->dataPtr->vel_pid[j]) || this->extractPIDFromParameters(
+          joint_info.command_interfaces[i].name, joint_name, this->dataPtr->vel_pid[j]);
+
+        if (this->dataPtr->is_vel_pid[j]) {
+          RCLCPP_INFO_STREAM(
+            this->nh_->get_logger(), "Found velocity PIDs for joint: " << joint_name << "!");
+          this->dataPtr->velocity_control_method_ = VELOCITY_PID;
         }
+
         this->dataPtr->command_interfaces_.emplace_back(
           joint_name,
           hardware_interface::HW_IF_VELOCITY,
@@ -641,19 +735,13 @@ GazeboSystem::perform_command_mode_switch(
       if (interface_name ==
         (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION))
       {
-        if (!this->dataPtr->is_pos_pid[j]) {
-          this->dataPtr->joint_control_methods_[j] |= POSITION;
-        } else {
-          this->dataPtr->joint_control_methods_[j] |= POSITION_PID;
-        }
+        this->dataPtr->joint_control_methods_[j] |=
+          static_cast<ControlMethod_>(this->dataPtr->position_control_method_);
       } else if (interface_name == // NOLINT
         (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY))
       {
-        if (!this->dataPtr->is_vel_pid[j]) {
-          this->dataPtr->joint_control_methods_[j] |= VELOCITY;
-        } else {
-          this->dataPtr->joint_control_methods_[j] |= VELOCITY_PID;
-        }
+        this->dataPtr->joint_control_methods_[j] |=
+          static_cast<ControlMethod_>(this->dataPtr->velocity_control_method_);
       } else if (interface_name == // NOLINT
         (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT))
       {

gazebo_ros2_control_demos/config/cart_controller_position_with_pids.yaml

@@ -0,0 +1,20 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    position_controller:
+      type: position_controllers/JointGroupPositionController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+position_controller:
+  ros__parameters:
+    joints:
+      - slider_to_cart
+
+gazebo_ros2_control:
+  ros__parameters:
+    pid_gains:
+      position:
+        slider_to_cart: {kp:  100.0, kd: 10.0, ki:  1.0, max_integral_error: 10000.0}