From 5b7b7f18a584f812b8cc718e4964730a302135d3 Mon Sep 17 00:00:00 2001
From: Christoph Froehlich <christoph.froehlich@ait.ac.at>
Date: Sun, 31 Dec 2023 01:06:21 +0000
Subject: [PATCH] Add option for not holding joints if not claimed

---
 .../include/gazebo_ros2_control/gazebo_system.hpp  |  3 +++
 .../src/gazebo_ros2_control_plugin.cpp             | 11 +++++++++++
 gazebo_ros2_control/src/gazebo_system.cpp          | 14 +++++++++++++-
 3 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
index 8230a4ec..059f11b4 100644
--- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
+++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
@@ -88,6 +88,9 @@ class GazeboSystem : public GazeboSystemInterface
 
   /// \brief Private data class
   std::unique_ptr<GazeboSystemPrivate> dataPtr;
+
+  // Should hold the joints if no control_mode is active
+  bool hold_joints_ = true;
 };
 
 }  // namespace gazebo_ros2_control
diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
index b0a8bb07..d4a776a0 100644
--- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
+++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
@@ -182,6 +182,12 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
   } else {
     impl_->robot_description_node_ = "robot_state_publisher";  // default
   }
+  // Hold joints if no control mode is active?
+  bool hold_joints = true;  // default
+  if (sdf->HasElement("hold_joints")) {
+    hold_joints =
+      sdf->GetElement("hold_joints")->Get<bool>();
+  }
 
   // There's currently no direct way to set parameters to the plugin's node
   // So we have to parse the plugin file manually and set it to the node's context.
@@ -306,6 +312,11 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
     RCLCPP_DEBUG(
       impl_->model_nh_->get_logger(), "Loaded hardware interface %s!",
       robot_hw_sim_type_str_.c_str());
+    try {
+      node_ros2->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints));
+    } catch (const std::exception & e) {
+      RCLCPP_ERROR(impl_->model_nh_->get_logger(), "%s", e.what());
+    }
     if (!gazeboSystem->initSim(
         node_ros2,
         impl_->parent_model_,
diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp
index a9292bd9..569c4b39 100644
--- a/gazebo_ros2_control/src/gazebo_system.cpp
+++ b/gazebo_ros2_control/src/gazebo_system.cpp
@@ -128,6 +128,18 @@ bool GazeboSystem::initSim(
     return false;
   }
 
+  try {
+    hold_joints_ = this->nh_->get_parameter("hold_joints").as_bool();
+  } catch (const std::exception & e) {
+    RCLCPP_WARN(
+      this->nh_->get_logger(),
+      "Parameter 'hold_joints' not found, with error %s", e.what());
+    RCLCPP_WARN_STREAM(
+      this->nh_->get_logger(), "Using default value: " << hold_joints_);
+  }
+  RCLCPP_DEBUG_STREAM(
+    this->nh_->get_logger(), "hold_joints (system): " << hold_joints_ << std::endl);
+
   registerJoints(hardware_info, parent_model);
   registerSensors(hardware_info, parent_model);
 
@@ -601,7 +613,7 @@ hardware_interface::return_type GazeboSystem::write(
         this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]);
       } else if (this->dataPtr->joint_control_methods_[j] & EFFORT) { // NOLINT
         this->dataPtr->sim_joints_[j]->SetForce(0, this->dataPtr->joint_effort_cmd_[j]);
-      } else if (this->dataPtr->is_joint_actuated_[j]) {
+      } else if (this->dataPtr->is_joint_actuated_[j] && hold_joints_) {
         // Fallback case is a velocity command of zero (only for actuated joints)
         this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0);
       }