Fix position_proportional_gain and update docs

ros-controls · Aug 22, 2024 · 89744ff · 89744ff
1 parent 6c8f5ee
commit 89744ff
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 7 deletions.
diff --git a/doc/index.rst b/doc/index.rst
@@ -141,6 +141,7 @@ The *gz_ros2_control* ``<plugin>`` tag also has the following optional child ele
 * ``<parameters>``: A YAML file with the configuration of the controllers. This element can be given multiple times to load multiple files.
 * ``<hold_joints>``: if set to true (default), it will hold the joints' position if their interface was not claimed, e.g., the controller hasn't been activated yet.
 * ``<controller_manager_name>``: Set controller manager name (default: ``controller_manager``)
+* ``<position_proportional_gain>``: Set the proportional gain. (default: 0.1) This determines the setpoint for a position-controlled joint ``joint_velocity = joint_position_error * position_proportional_gain``.
 
 Additionally, one can specify a namespace and remapping rules, which will be forwarded to the controller_manager and loaded controllers. Add the following ``<ros>`` section:
 

diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp
@@ -315,6 +315,11 @@ void GazeboSimROS2ControlPlugin::Configure(
     hold_joints =
       sdfPtr->GetElement("hold_joints")->Get<bool>();
   }
+  double position_proportional_gain = 0.1;  // default
+  if (sdfPtr->HasElement("position_proportional_gain")) {
+    position_proportional_gain =
+      sdfPtr->GetElement("position_proportional_gain")->Get<double>();
+  }
 
   if (sdfPtr->HasElement("ros")) {
     sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros");
@@ -400,6 +405,31 @@ void GazeboSimROS2ControlPlugin::Configure(
       e.what());
   }
 
+  try {
+    this->dataPtr->node_->declare_parameter("position_proportional_gain",
+        rclcpp::ParameterValue(position_proportional_gain));
+  } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(),
+        "Parameter 'position_proportional_gain' has already been declared, %s",
+      e.what());
+  } catch (const rclcpp::exceptions::InvalidParametersException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(),
+        "Parameter 'position_proportional_gain' has invalid name, %s",
+      e.what());
+  } catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(),
+        "Parameter 'position_proportional_gain' value is invalid, %s",
+      e.what());
+  } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+    RCLCPP_ERROR(
+      this->dataPtr->node_->get_logger(),
+        "Parameter 'position_proportional_gain' value has wrong type, %s",
+      e.what());
+  }
+
   std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
     std::make_unique<gz_ros2_control::GZResourceManager>(this->dataPtr->node_, _ecm, enabledJoints);
 

diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp
@@ -200,14 +200,30 @@ bool GazeboSimSystem::initSim(
 
   this->dataPtr->joints_.resize(this->dataPtr->n_dof_);
 
-  constexpr double default_gain = 0.1;
-
   try {
-    this->dataPtr->position_proportional_gain_ = this->nh_->declare_parameter<double>(
-      "position_proportional_gain", default_gain);
-  } catch (rclcpp::exceptions::ParameterAlreadyDeclaredException & ex) {
-    this->nh_->get_parameter(
-      "position_proportional_gain", this->dataPtr->position_proportional_gain_);
+    this->dataPtr->position_proportional_gain_ =
+      this->nh_->get_parameter("position_proportional_gain").as_double();
+  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'position_proportional_gain' not initialized, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(),
+        "Using default value: " << this->dataPtr->position_proportional_gain_);
+  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'position_proportional_gain' not declared, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(),
+        "Using default value: " << this->dataPtr->position_proportional_gain_);
+  } catch (rclcpp::ParameterTypeException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'position_proportional_gain' has wrong type: %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(),
+        "Using default value: " << this->dataPtr->position_proportional_gain_);
   }
 
   RCLCPP_INFO_STREAM(