Add hold_joints parameter (#213)

Signed-off-by: Alejandro Hernández Cordero <[email protected]>
ros-controls · Jan 3, 2024 · cf7adb1 · cf7adb1
1 parent 3a44b0b
commit cf7adb1
Show file tree

Hide file tree

Showing 3 changed files with 58 additions and 1 deletion.
diff --git a/doc/index.rst b/doc/index.rst
@@ -153,6 +153,7 @@ robot hardware interfaces between *ros2_control* and Gazebo.
 The *gz_ros2_control* ``<plugin>`` tag also has the following optional child elements:
 
 * ``<parameters>``: YAML file with the configuration of the controllers
+* ``<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.
 
 Default gz_ros2_control Behavior
 -----------------------------------------------------------

diff --git a/gz_ros2_control/src/gz_ros2_control_plugin.cpp b/gz_ros2_control/src/gz_ros2_control_plugin.cpp
@@ -299,6 +299,13 @@ void GazeboSimROS2ControlPlugin::Configure(
   std::string controllerManagerNodeName{"controller_manager"};
   std::string ns = "/";
 
+  // Hold joints if no control mode is active?
+  bool hold_joints = true;  // default
+  if (sdfPtr->HasElement("hold_joints")) {
+    hold_joints =
+      sdfPtr->GetElement("hold_joints")->Get<bool>();
+  }
+
   if (sdfPtr->HasElement("ros")) {
     sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros");
 
@@ -417,6 +424,27 @@ void GazeboSimROS2ControlPlugin::Configure(
         ex.what());
       continue;
     }
+
+    try {
+      this->dataPtr->node_->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
+    } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException & e) {
+      RCLCPP_ERROR(
+        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has already been declared, %s",
+        e.what());
+    } catch (const rclcpp::exceptions::InvalidParametersException & e) {
+      RCLCPP_ERROR(
+        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has invalid name, %s",
+        e.what());
+    } catch (const rclcpp::exceptions::InvalidParameterValueException & e) {
+      RCLCPP_ERROR(
+        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value is invalid, %s",
+        e.what());
+    } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
+      RCLCPP_ERROR(
+        this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s",
+        e.what());
+    }
+
     if (!gzSimSystem->initSim(
         this->dataPtr->node_,
         enabledJoints,

diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp
@@ -175,6 +175,9 @@ class gz_ros2_control::GazeboSimSystemPrivate
 
   /// \brief Gain which converts position error to a velocity command
   double position_proportional_gain_;
+
+  // Should hold the joints if no control_mode is active
+  bool hold_joints_ = true;
 };
 
 namespace gz_ros2_control
@@ -196,6 +199,31 @@ bool GazeboSimSystem::initSim(
 
   this->dataPtr->update_rate = &update_rate;
 
+  try {
+    this->dataPtr->hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool();
+  } catch (rclcpp::exceptions::ParameterUninitializedException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' not initialized, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' not declared, with error %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << this->dataPtr->hold_joints_);
+  } catch (rclcpp::ParameterTypeException & ex) {
+    RCLCPP_ERROR(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' has wrong type: %s", ex.what());
+    RCLCPP_WARN_STREAM(
+      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);
+
+
   RCLCPP_DEBUG(this->nh_->get_logger(), "n_dof_ %lu", this->dataPtr->n_dof_);
 
   this->dataPtr->joints_.resize(this->dataPtr->n_dof_);
@@ -658,7 +686,7 @@ hardware_interface::return_type GazeboSimSystem::write(
         *jointEffortCmd = sim::components::JointForceCmd(
           {this->dataPtr->joints_[i].joint_effort_cmd});
       }
-    } else if (this->dataPtr->joints_[i].is_actuated) {
+    } else if (this->dataPtr->joints_[i].is_actuated && this->dataPtr->hold_joints_) {
       // Fallback case is a velocity command of zero (only for actuated joints)
       double target_vel = 0.0;
       auto vel =