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 move(int targetPosition, int speed, int force) override{ - // Ensure only one movement operation is active at a time + std::tuple 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(&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(&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 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 communication_thread_is_running_; std::mutex resource_mutex_; void background_task(); + void performRegularOperations(); static constexpr double NO_NEW_CMD_ = std::numeric_limits::quiet_NaN();