From 9021f754c937742835e7a318fbc645d07278d701 Mon Sep 17 00:00:00 2001
From: ligerfotis <ligerfotis@gmail.com>
Date: Tue, 16 Apr 2024 13:47:24 +0200
Subject: [PATCH] fix last state initial state update for the gripper

---
 src/servo_keyboard/package.xml                |  7 ------
 .../src/servo_keyboard_input.cpp              | 24 +++++++++----------
 2 files changed, 12 insertions(+), 19 deletions(-)

diff --git a/src/servo_keyboard/package.xml b/src/servo_keyboard/package.xml
index 391b053..6f3de4e 100644
--- a/src/servo_keyboard/package.xml
+++ b/src/servo_keyboard/package.xml
@@ -11,18 +11,11 @@
 
   <!-- Dependencies -->
   <depend>rclcpp</depend>
-<<<<<<<< HEAD:src/servo_keyboard/package.xml
   <depend>control_msgs</depend>
   <depend>geometry_msgs</depend>
   <build_depend>controller_manager_msgs</build_depend>
   <exec_depend>controller_manager_msgs</exec_depend>
 
-========
-  <depend>rclcpp_lifecycle</depend>
-  <depend>hardware_interface</depend>
-  <depend>pluginlib</depend>
->>>>>>>> origin/main:src/robotiq_2f/robotiq_2f_interface/package.xml
-
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 
diff --git a/src/servo_keyboard/src/servo_keyboard_input.cpp b/src/servo_keyboard/src/servo_keyboard_input.cpp
index 04cee63..45f2fd1 100644
--- a/src/servo_keyboard/src/servo_keyboard_input.cpp
+++ b/src/servo_keyboard/src/servo_keyboard_input.cpp
@@ -125,7 +125,7 @@ private:
 
   void handlePlusPress();
   void handleMinusPress();
-
+  void jointStateCallback(sensor_msgs::msg::JointState::SharedPtr msg); // Declaration of jointStateCallback
   void publishGripperCommand(double finger_joint_angle);
 
   rclcpp::Node::SharedPtr nh_;
@@ -147,7 +147,7 @@ KeyboardServo::KeyboardServo()
   : frame_to_publish_(BASE_FRAME_ID),
     joint_vel_cmd_(1.0),
     gripper_speed_multiplier_(0.01), // Example value, adjust as needed
-    last_finger_joint_angle_(0.0),  // Example value, adjust as needed
+//    last_finger_joint_angle_(0.0),  // Example value, adjust as needed
     gripper_lower_limit_(0.0),      // Example value, adjust as needed
     gripper_upper_limit_(0.70)       // Example value, adjust as needed
 {
@@ -163,17 +163,17 @@ KeyboardServo::KeyboardServo()
 
 KeyboardReader input;
 
-// Define callback function for joint states subscriber
-void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg)
+// Implement the jointStateCallback function
+void KeyboardServo::jointStateCallback(sensor_msgs::msg::JointState::SharedPtr msg)
 {
-    // Find index of the finger joint in the joint state message
-    auto it = std::find(msg->name.begin(), msg->name.end(), "finger_joint");
-    if (it != msg->name.end())
-    {
-        size_t index = std::distance(msg->name.begin(), it);
-        // Update last_finger_joint_angle_ with the corresponding value from the message
-        last_finger_joint_angle_ = msg->position[index];
-    }
+  // Find the index of the finger joint in the message
+  auto it = std::find(msg->name.begin(), msg->name.end(), "finger_joint");
+  if (it != msg->name.end())
+  {
+    size_t index = std::distance(msg->name.begin(), it);
+    // Set the last finger joint angle based on the received message
+    last_finger_joint_angle_ = msg->position[index];
+  }
 }
 
 void KeyboardServo::publishGripperCommand(double finger_joint_angle)