From 1e99f38dafc58b9810ef05fed4cdda4a792bcc7a Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Mon, 15 Apr 2024 13:53:41 +0200
Subject: [PATCH] mock system working

---
 .../MockRobotiq2fSocketAdapter.hpp            | 38 ++++++++++++-------
 .../hardware_interface.hpp                    |  1 +
 2 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp
index bed6529..a3f4288 100644
--- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp
+++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp
@@ -48,23 +48,37 @@ public:
     }
 
     void activate(bool autoCalibrate = true) override {
-        isActive.store(true);
+        isActive.store(true);  // Ensure we set active before moving
+        if (!autoCalibrate) {
+            // Simulate a calibration sequence
+            move_and_wait(0, 64, 1);
+            move_and_wait(255, 64, 1);
+            move_and_wait(0, 64, 1);
+        }
     }
 
     void deactivate() override {
         isActive.store(false);
     }
 
-    std::tuple<bool, int> move(int targetPosition, int speed, int force) override{
-        // Ensure only one movement operation is active at a time
+    std::tuple<bool, int> move(int targetPosition, int speed, int force) {
+        // Start a new movement operation in a background thread
         if (movementThread && movementThread->joinable()) {
-            movementThread->join(); // Wait for any ongoing movement to complete
+            movementThread->join(); // Ensure no concurrent movements
         }
 
-        // Start a new movement operation in a background thread
-        movementThread = std::make_unique<std::thread>(&MockRobotiq2fSocketAdapter::simulateMovement, this, targetPosition, speed, force);
+        gripperSpeed.store(speed);  // Simulate immediate speed update
+        gripperForce.store(force);  // Simulate immediate force update
 
-        return std::make_tuple(true, targetPosition); // Immediately return success and the target position
+        movementThread = std::make_unique<std::thread>(&MockRobotiq2fSocketAdapter::simulateMovement, this, targetPosition, speed, force);
+        return std::make_tuple(true, targetPosition);
+    }
+
+    void move_and_wait(int targetPosition, int speed, int force) {
+        auto [success, pos] = move(targetPosition, speed, force);
+        if (movementThread && movementThread->joinable()) {
+            movementThread->join();  // Wait for movement to complete
+        }
     }
 
 private:
@@ -77,17 +91,15 @@ private:
     std::unique_ptr<std::thread> movementThread;
 
     void simulateMovement(int targetPosition, int speed, int force) {
-        int step = (targetPosition > gripperPosition) ? 1 : -1;
+        int step = (targetPosition > gripperPosition.load()) ? 1 : -1;
 
         // Simulate moving towards the target position
-        while (gripperPosition != targetPosition) {
-            gripperPosition += step;
+        while (gripperPosition.load() != targetPosition) {
+            gripperPosition.fetch_add(step);
             std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Simulate time to move a step
         }
 
-        // Here, you simulate setting the speed and force, though for simplicity, we're just logging it
-        // In a real mock, you might update some internal state to reflect these changes
-        std::cout << "Simulated movement to position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
+        std::cout << "Reached position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
     }
 };
 } // end robotiq_interface
diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp
index 9417b5e..4e3e0ab 100644
--- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp
+++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp
@@ -62,6 +62,7 @@ protected: // Likely changes the access to protected from private
     std::atomic<bool> communication_thread_is_running_;
     std::mutex resource_mutex_;
     void background_task();
+    void performRegularOperations();
 
     static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();