From 1056b52d0bf020140a6e0be50feb4bc4b598894f Mon Sep 17 00:00:00 2001
From: Captain Yoshi <captain.yoshisaur@gmail.com>
Date: Fri, 15 Dec 2023 16:02:01 -0500
Subject: [PATCH] Optionnal hand_frame_control ros param initialization

---
 cartesian_force_controller/README.md                     | 1 +
 .../cartesian_force_controller.hpp                       | 9 +++++++++
 2 files changed, 10 insertions(+)

diff --git a/cartesian_force_controller/README.md b/cartesian_force_controller/README.md
index f3ed2dff..d264ebf2 100644
--- a/cartesian_force_controller/README.md
+++ b/cartesian_force_controller/README.md
@@ -24,6 +24,7 @@ my_cartesian_force_controller:
     end_effector_link: "tool0"
     robot_base_link: "base_link"
     ft_sensor_ref_link: "sensor_link"
+    hand_frame_control: true
     joints:
     - joint1
     - joint2
diff --git a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp
index 107d6499..aa0a3be3 100644
--- a/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp
+++ b/cartesian_force_controller/include/cartesian_force_controller/cartesian_force_controller.hpp
@@ -69,6 +69,15 @@ init(HardwareInterface* hw, ros::NodeHandle& nh)
     return false;
   }
 
+  if(nh.hasParam("hand_frame_control"))
+  {
+    if (!nh.getParam("hand_frame_control",m_hand_frame_control))
+    {
+      ROS_ERROR_STREAM("Failed to load " << nh.getNamespace() + "/hand_frame_control" << " from parameter server");
+      return false;
+    }
+  }
+
   // Make sure sensor link is part of the robot chain
   if(!Base::robotChainContains(m_ft_sensor_ref_link))
   {