extracted the tool communication launch

added new Robotiq package for hardware interface with socket adapter

src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotiq_2f_interface)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp

@@ -0,0 +1,356 @@
+// Robotiq2fSocketAdapter.hpp
+
+#ifndef ROBOTIQ2F_SOCKET_ADAPTER_HPP
+#define ROBOTIQ2F_SOCKET_ADAPTER_HPP
+
+#include <mutex>
+#include <thread>
+
+#include <string>
+#include <map>
+#include <vector>
+#include <sys/time.h>
+#include <sstream>
+#include <tuple>
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#include <unistd.h> // For close()
+#include <cstring> // For memset()
+#include <sys/select.h>  // For select() system call
+
+#include <stdexcept> // For std::runtime_error
+#include <cerrno> // For errno
+#include <iostream>
+
+// Enum for Gripper Status
+enum class GripperStatus {
+    RESET = 0,
+    ACTIVATING = 1,
+    // UNUSED = 2, // This value is currently not used by the gripper firmware
+    ACTIVE = 3
+};
+
+// Enum for Object Status
+enum class ObjectStatus {
+    MOVING = 0,
+    STOPPED_OUTER_OBJECT = 1,
+    STOPPED_INNER_OBJECT = 2,
+    AT_DEST = 3
+};
+
+class Robotiq2fSocketAdapter {
+public:
+
+    Robotiq2fSocketAdapter() ;
+    ~Robotiq2fSocketAdapter();
+
+    // Connection management
+    /**
+     * Establishes a TCP connection to the Robotiq gripper.
+     *
+     * Attempts to open a socket and connect to the specified hostname and port. If the adapter
+     * is already connected, it will disconnect and attempt to reconnect.
+     *
+     * @param hostname The IP address or hostname of the Robotiq gripper.
+     * @param port The port number on which the gripper is listening for connections.
+     * @return True if the connection was successfully established, False otherwise.
+     *
+     * @throws std::runtime_error if socket creation fails.
+     */
+    bool connect(const std::string& hostname, int port);
+    /**
+    * Closes the connection to the Robotiq gripper.
+    *
+    * Safely shuts down and closes the current socket connection to the gripper. If the socket
+    * is already closed or was never connected, this method has no effect. This method is
+    * thread-safe and can be called concurrently with other operations on the adapter.
+    */
+    void disconnect();
+
+
+    /**
+    * Sets multiple gripper variables using a map of variable names to integer/double values.
+    *
+    * This method allows setting multiple configuration parameters of the gripper simultaneously.
+    * The variable names must match the expected variable identifiers used by the gripper.
+    * Integer/double values are used directly in the command sent to the gripper.
+    *
+    * @param variableMap A map where each key is a string representing the variable name and
+    *        each value is an integer representing the variable's desired value.
+    * @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
+    *
+    * @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
+    */
+    bool setGripperVariables(const std::map<std::string, int>& variableMap);
+    bool setGripperVariables(const std::map<std::string, double>& variableMap);
+
+    /**
+    * Sets a single gripper variable.
+    *
+    * Allows setting a single configuration parameter of the gripper. The method overloads allow
+    * specifying the variable value as either an integer or a double, depending on the expected
+    * variable type.
+    *
+    * @param variable The variable name, matching the gripper's expected variable identifiers.
+    * @param value The desired value for the variable, either as an int or a double.
+    * @return True if the command was successfully sent and acknowledged by the gripper, False otherwise.
+    *
+    * @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
+    */
+    bool setGripperVariable(const std::string& variable, int value);
+    bool setGripperVariable(const std::string& variable, double value);
+
+    /**
+    * Retrieves the current value of a specified gripper variable.
+    *
+    * Sends a command to the gripper to report the current value of a specific configuration parameter.
+    * The method parses the gripper's response to extract the variable value.
+    *
+    * @param variable The variable name, matching the gripper's expected variable identifiers.
+    * @return The current value of the variable as an integer. Returns -1 if the command fails or
+    *         if the variable is not recognized by the gripper.
+    *
+    * @throws std::runtime_error if the adapter is not connected to the gripper, if sending the command fails,
+    *         or if the response from the gripper is malformed.
+    */
+    int getGripperVariable(const std::string& variable);
+
+    // Gripper status
+    bool isGripperActive();
+    int getGripperPosition();
+    int getGripperForce();
+    int force();
+    int speed();
+    int position();
+
+    /**
+    * Checks if the gripper is currently active.
+    *
+    * Queries the current state of the gripper to determine if it is active. This method interprets
+    * the gripper's status based on the last known communication and does not initiate a new request
+    * to the gripper.
+    *
+    * @return True if the gripper is in the ACTIVE state, False otherwise.
+    *
+    * @throws std::runtime_error if unable to retrieve the gripper's status, typically due to communication issues.
+    */
+    bool is_active();
+
+
+    // Activates the gripper and optionally performs auto-calibration
+    /**
+    * Activates the Robotiq gripper and optionally performs auto-calibration.
+    *
+    * This method sends an activation command to the gripper, bringing it from a RESET or INACTIVE
+    * state to an ACTIVE state. If autoCalibrate is set to true, it also initiates an auto-calibration
+    * procedure to determine the maximum and minimum positions of the gripper's motion range.
+    *
+    * @param autoCalibrate A boolean flag indicating whether to perform auto-calibration after
+    *        activation. Default is true.
+    *
+    * @note Activation and auto-calibration may take several seconds to complete. The method
+    *       waits for the gripper to acknowledge activation before returning.
+    *
+    * @throws std::runtime_error if the adapter is not connected to the gripper, if sending the
+    *         activation command fails, or if the gripper does not reach the expected state within
+    *         a reasonable timeframe.
+    */
+    void activate(bool autoCalibrate = true);
+    /**
+    * Performs auto-calibration of the gripper.
+    *
+    * This method initiates an auto-calibration sequence where the gripper fully opens and closes to determine
+    * its operational range. This is useful for ensuring accurate position control, especially if the mechanical
+    * setup of the gripper has been altered or if it is being used for the first time.
+    *
+    * @param log A boolean flag indicating whether to log the calibration results. Defaults to true. When set
+    *            to true, the final calibration values will be logged to the standard output.
+    *
+    * @note This method assumes the gripper is already activated. If the gripper is not in an active state,
+    *       the calibration sequence may fail. Ensure the gripper is activated before calling this method.
+    *
+    * @throws std::runtime_error if the adapter is not connected to the gripper, if the gripper is not in an
+    *         active state, or if the calibration sequence fails.
+    */
+    void autoCalibrate(bool log=true);
+
+    /**
+    * Deactivates the gripper.
+    *
+    * Sends a command to transition the gripper from an active state back to a reset state. This method
+    * is useful for safely shutting down the gripper or preparing it for a state change.
+    *
+    * @note This method should be called before disconnecting from the gripper or when the gripper is not
+    *       needed for an extended period to ensure it is left in a safe state.
+    *
+    * @throws std::runtime_error if the adapter is not connected to the gripper or if the deactivation command fails.
+    */
+    void deactivate();
+
+    // Move Commands
+    /**
+    * Commands the gripper to move to a specified position.
+    *
+    * Instructs the gripper to move to a given position with specified speed and force. The position,
+    * speed, and force parameters are automatically clipped to the valid range supported by the gripper.
+    *
+    * @param position The target position for the gripper, typically in the range [0, 255].
+    * @param speed The speed at which the gripper should move, typically in the range [0, 255].
+    * @param force The force to be applied by the gripper, typically in the range [0, 255].
+    * @return A tuple containing a boolean indicating success of the command, and an integer representing the clipped target position.
+    *
+    * @throws std::runtime_error if there's an error in sending the move command or if the adapter is not connected to a gripper.
+    */
+    std::tuple<bool, int> move(int position, int speed, int force);
+    /**
+    * Commands the gripper to move to a specified position and waits for the move to complete.
+    *
+    * This method instructs the gripper to move to the given position with specified speed and force,
+    * then blocks until the gripper acknowledges reaching the target position or stops moving due to
+    * detecting an object.
+    *
+    * @param position The target position for the gripper, typically in the range [0, 255].
+    * @param speed The speed at which the gripper should move, typically in the range [0, 255].
+    * @param force The force to be applied by the gripper, typically in the range [0, 255].
+    * @return A tuple containing the final position of the gripper as an integer and the object status
+    *         as an `ObjectStatus` enumeration value.
+    *
+    * @throws std::runtime_error if there's an error in sending the move command, if the adapter is not
+    *         connected to a gripper, or if the gripper does not reach the target position within a
+    *         reasonable time frame.
+    */
+    std::tuple<int, ObjectStatus> move_and_wait_for_pos(int position, int speed, int force);
+
+private:
+    // Custom deleter for the smart pointer managing the socket descriptor
+    struct SocketDeleter {
+        void operator()(int* ptr) {
+            if (ptr && *ptr >= 0) {
+                close(*ptr);
+            }
+            delete ptr;
+        }
+    };
+
+    // Smart pointer to manage the socket descriptor with the custom deleter
+    std::unique_ptr<int, SocketDeleter> _sockfd;
+    std::mutex socketMutex; // Mutex for socket access synchronization
+
+    // bounds of the encoded gripper states
+    int _min_position = 0;
+    int _max_position = 255;
+    int _min_speed = 0;
+    int _max_speed = 255;
+    int _min_force = 0;
+    int _max_force = 255;
+
+    int open_position = _min_position;
+    int closed_position = _max_position;
+
+
+    /**
+    * Sends a command to the connected Robotiq gripper.
+    *
+    * Constructs and sends a command string to the gripper over the established socket connection.
+    * This method ensures thread-safe access to the socket and handles low-level socket write operations.
+    *
+    * @param cmd The command string to be sent to the gripper. Must conform to the gripper's protocol.
+    * @return True if the command was successfully sent, False if there was an error sending the command.
+    *
+    * @throws std::runtime_error if attempted on a disconnected socket.
+    */
+    bool sendCommand(const std::string& cmd);
+
+    /**
+    * Receives a response from the Robotiq gripper.
+    *
+    * Waits for and reads a response from the gripper within a specified timeout period. This method
+    * is blocking but allows specifying a timeout to prevent indefinite waiting. It ensures thread-safe
+    * access to the socket and handles low-level socket read operations.
+    *
+    * @param timeout_ms The timeout in milliseconds to wait for a response. Defaults to 2000 milliseconds.
+    * @return A string containing the response from the gripper. If the operation times out or fails, an empty string is returned.
+    *
+    * @throws std::runtime_error if attempted on a disconnected socket or if a select() call fails.
+    */
+    std::string receiveResponse(int timeout_ms=2000);
+
+
+    /**
+    * Checks if the received data is an acknowledgement message from the gripper.
+    *
+    * This method parses the received data string to determine if it matches the expected acknowledgement
+    * pattern or message, indicating that a previously sent command was received and executed by the gripper.
+    *
+    * @param data The raw string data received from the gripper.
+    * @return True if the data represents an acknowledgement message, False otherwise.
+    */
+    bool isAck(const std::string& data);
+
+    // Helper Functions
+    /**
+    * Clips a variable to ensure it falls within a specified range.
+    *
+    * This helper function ensures that a given value does not exceed the minimum and maximum bounds
+    * defined for it. It is particularly useful for sanitizing command parameters like position, speed,
+    * and force values before sending them to the gripper.
+    *
+    * @param min_value The minimum allowable value for the variable.
+    * @param variable The current value of the variable to be clipped.
+    * @param max_value The maximum allowable value for the variable.
+    * @return The clipped value, constrained within the specified min and max bounds.
+    */
+    int clip_val(int min_value, int variable, int max_value);
+    /**
+    * Blocks until the gripper reaches a specified status.
+    *
+    * This method polls the gripper's status at regular intervals until it matches the expected status
+    * or until a timeout occurs. It is used internally to synchronize actions like activation, deactivation,
+    * and calibration with the gripper's state transitions.
+    *
+    * @param expectedStatus The GripperStatus enumeration value the method waits for the gripper to achieve.
+    *
+    * @throws std::runtime_error if the gripper does not reach the expected status within a reasonable time frame,
+    *         indicating a possible communication issue or a failure in executing the command.
+    */
+    void waitForGripperStatus(GripperStatus expectedStatus);
+
+    // Resets the gripper
+    /**
+    * Resets the gripper to its initial state.
+    *
+    * Sends a command to the gripper to transition it to the RESET state. This is often used as a first step
+    * in a sequence of commands that prepare the gripper for operation, ensuring it starts from a known state.
+    *
+    * @note This method can be used to recover the gripper from error states or to prepare it for reactivation.
+    *
+    * @throws std::runtime_error if the adapter is not connected to the gripper or if the reset command fails.
+    */
+    void reset();
+
+    // Define the command strings and encoding used for communication
+    const std::string SET_COMMAND = "SET";
+    const std::string GET_COMMAND = "GET";
+    const std::string ACK_MESSAGE = "ack";
+
+    // Commands
+    const std::string CMD_ACT = "ACT";  // Activation
+    const std::string CMD_GTO = "GTO";  // GoTo
+    const std::string CMD_ATR = "ATR";  // Automatic Release
+    const std::string CMD_ADR = "ADR";  // Advanced Control
+    const std::string CMD_FOR = "FOR";  // Force
+    const std::string CMD_SPE = "SPE";  // Speed
+    const std::string CMD_POS = "POS";  // Position
+    const std::string CMD_STA = "STA";  // Status
+    const std::string CMD_PRE = "PRE";  // Position Request
+    const std::string CMD_OBJ = "OBJ";  // Object Detection
+    const std::string CMD_FLT = "FLT";  // Fault
+
+    // Constants buffer sizes
+    const size_t BUFFER_SIZE = 1024; // Adjust as necessary
+};
+
+#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP

src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp

@@ -0,0 +1,63 @@
+// hardware_interface.hpp
+
+#ifndef ROBOTIQ2F_HARDWARE_INTERFACE_HPP
+#define ROBOTIQ2F_HARDWARE_INTERFACE_HPP
+
+#include <chrono>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <vector>
+
+#include <Robotiq2fSocketAdapter.hpp>
+
+#include <hardware_interface/actuator_interface.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+#include <urdf/model.h>
+
+namespace robotiq_driver
+{
+class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
+{
+public:
+  // Constructors
+  ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface();
+  ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface();
+  // ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr<DriverFactory> driver_factory);
+
+  // Lifecycle Management
+  ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
+  ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+  ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  // State and Command Interfaces
+  ROBOTIQ_DRIVER_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+  ROBOTIQ_DRIVER_PUBLIC std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  // Data Access (Read/Write)
+  ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+  ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+protected: // Likely changes the access to protected from private
+  // Members for driver interaction
+  std::unique_ptr<Robotiq2fSocketAdapter> socket_adapter_;
+  // std::unique_ptr<DriverFactory> driver_factory_;
+
+  // Background communication thread
+  std::thread communication_thread_;
+  std::atomic<bool> communication_thread_is_running_;
+  void background_task();
+
+  // Gripper State Tracking
+  double gripper_closed_pos_ = 0.0;
+  double gripper_position_ = 0.0;
+  double gripper_velocity_ = 0.0;
+  double gripper_position_command_ = 0.0;
+  // ... other internal state variables ...
+};
+}
+
+#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP

src/robotiq_2f/robotiq_2f_interface/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>robotiq_2f_interface</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>hardware_interface</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp

@@ -0,0 +1,369 @@
+#include "Robotiq2fSocketAdapter.hpp"
+
+
+Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : _sockfd(new int(-1)) {
+}
+
+
+Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
+    disconnect();  // Ensure the socket is closed properly.
+}
+
+// Connection and disconnection
+bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
+    std::lock_guard<std::mutex> lock(socketMutex);
+    if (*_sockfd >= 0) {
+        std::cerr << "Already connected. Disconnecting to reconnect.\n";
+        disconnect();
+    }
+
+    int sock = socket(AF_INET, SOCK_STREAM, 0);
+    if (sock < 0) {
+        std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
+        return false;
+    }
+    *_sockfd = sock;
+
+    sockaddr_in serv_addr{};
+    memset(&serv_addr, 0, sizeof(serv_addr));
+    serv_addr.sin_family = AF_INET;
+    serv_addr.sin_port = htons(port);
+
+    if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) {
+        std::cerr << "Invalid address: " << hostname << "\n";
+        close(*_sockfd);
+        *_sockfd = -1;
+        return false;
+    }
+
+    if (::connect(_sockfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
+        std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
+        close(*_sockfd);
+        *_sockfd = -1;
+        return false;
+    }
+
+    return true;
+}
+
+void Robotiq2fSocketAdapter::disconnect() {
+    std::lock_guard<std::mutex> lock(socketMutex);
+    if (*_sockfd >= 0) {
+        if (close(*_sockfd) == -1) {
+            std::cerr << "Error closing socket: " << strerror(errno) << "\n";
+        } else {
+            std::cout << "Disconnected successfully.\n";
+        }
+        *_sockfd = -1;
+    }
+}
+
+// Utility methods
+bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
+    std::lock_guard<std::mutex> lock(socketMutex);
+    if (*_sockfd < 0) {
+        std::cerr << "Attempted to send command on a disconnected socket.\n";
+        return false;
+    }
+
+    ssize_t result = send(*_sockfd, cmd.c_str(), cmd.length(), 0);
+    if (result < 0) {
+        std::cerr << "Failed to send command: " << strerror(errno) << "\n";
+        return false;
+    }
+
+    return true;
+}
+
+std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) {
+    std::lock_guard<std::mutex> lock(socketMutex);
+    if (*_sockfd < 0) {
+        std::cerr << "Attempted to receive response on a disconnected socket.\n";
+        return "";
+    }
+
+    std::string result;
+    char buffer[BUFFER_SIZE];
+    bool complete_response = false;
+
+    timeval tv;
+    tv.tv_sec = timeout_ms / 1000;
+    tv.tv_usec = (timeout_ms % 1000) * 1000;
+
+    // Loop to receive data until a complete response is obtained or a timeout occurs
+    while (!complete_response) {
+        fd_set readfds;
+        FD_ZERO(&readfds);
+        FD_SET(*_sockfd, &readfds);
+
+        int activity = select(*_sockfd + 1, &readfds, nullptr, nullptr, &tv);
+        if (activity == -1) {
+            std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
+            return "";
+        } else if (activity == 0) {
+            std::cerr << "Timeout occurred while waiting for response.\n";
+            return "";
+        }
+
+        ssize_t bytes_read = recv(*_sockfd, buffer, BUFFER_SIZE - 1, 0);
+        if (bytes_read < 0) {
+            if (errno != EAGAIN && errno != EWOULDBLOCK) {
+                std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
+            }
+            return "";
+        } else if (bytes_read == 0) {
+            std::cerr << "Connection closed by peer.\n";
+            return "";
+        } else {
+            buffer[bytes_read] = '\0';
+            result += buffer;
+
+            // Check if we have a complete response:
+            if (result.find("\n") != std::string::npos) {
+                complete_response = true;
+            }
+        }
+    }
+
+    return result;
+}
+
+bool Robotiq2fSocketAdapter::isAck(const std::string& data) {
+    return data == ACK_MESSAGE;
+}
+
+int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
+    return std::max(min_value, std::min(value, max_value));
+}
+
+void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) {
+    int retries = 0;
+    int status = getGripperVariable(CMD_STA);
+    while (static_cast<GripperStatus>(status) != expectedStatus && retries < MAX_RETRIES) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(RETRY_DELAY_MS));
+        status = getGripperVariable(CMD_STA);
+        retries++;
+    }
+    if (retries >= MAX_RETRIES) {
+        throw std::runtime_error("Gripper failed to reach expected status.");
+    }
+}
+
+
+// Gripper variable Getter and Setter
+bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, int>& variableMap) {
+    return setGripperVariables(reinterpret_cast<const std::map<std::string, double>&>(variableMap));
+}
+
+bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, double>& variableMap) {
+    // Build the command string
+    std::string cmd = SET_COMMAND;
+    for (const auto& [variable, value] : variableMap) {
+        std::stringstream ss;
+        ss << value;
+        cmd += " " + commandVariables.at(variable) + " " + ss.str();
+    }
+    cmd += "\n";
+
+    // Send the command and receive the response
+    std::lock_guard<std::mutex> lock(socketMutex);
+    if (*_sockfd < 0) {
+        throw std::runtime_error("Cannot set variables on a disconnected socket.");
+        return false;
+    }
+
+    if (!sendCommand(cmd)) {
+        throw std::runtime_error("[setGripperVariables] Sending command failed.");
+        return false;
+    }
+
+    std::string response = receiveResponse();
+    return isAck(response);
+}
+
+bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, int value) {
+    return setGripperVariable(variable, static_cast<double>(value)); // Convert int to double
+}
+
+bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) {
+    std::stringstream ss;
+    ss << value;  // Convert the value to a string
+
+    std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n";
+
+    std::lock_guard<std::mutex> lock(socketMutex);
+    if (*_sockfd < 0) {
+        throw std::runtime_error("Cannot set variables on a disconnected socket.");
+        return false;
+    }
+
+    if (!sendCommand(cmd)) {
+        throw std::runtime_error("[setGripperVariable] Sending command failed.");
+        return false; // Sending command failed
+    }
+
+    std::string response = receiveResponse();
+    return isAck(response);
+}
+
+int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
+    std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n";
+
+    std::lock_guard<std::mutex> lock(socketMutex);
+    if (*_sockfd < 0) {
+        throw std::runtime_error("Cannot get variables on a disconnected socket.");
+        return -1;
+    }
+
+    if (!sendCommand(cmd)) {
+        throw std::runtime_error("[getGripperVariable] Sending command failed.");
+        return -1; // Error sending command
+    }
+
+    std::string response = receiveResponse();
+
+    // Parse the response (assuming format: "variable_name value")
+    std::istringstream iss(response);
+    std::string var_name, value_str;
+
+    if (!(iss >> var_name >> value_str)) {
+        throw std::runtime_error("Invalid gripper response format.");
+        return -1;
+    }
+
+    if (var_name != variable) {
+        throw std::runtime_error("Unexpected response - variable name mismatch.");
+        return -1;
+    }
+
+    try {
+        return std::stoi(value_str);
+    } catch (const std::exception& ex) {
+        std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
+        return -1;
+    }
+}
+
+// Activation, deactivation and reset of the Gripper
+void Robotiq2fSocketAdapter::activate(bool autoCalibrate) {
+    reset(); // Always start with a reset to ensure a known state
+    setGripperVariable(CMD_ACT, 1); // Activate the gripper
+    waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active
+
+    if (autoCalibrate) {
+        auto_calibrate();
+    }
+}
+
+void Robotiq2fSocketAdapter::reset() {
+    setGripperVariable(CMD_ACT, 0);
+    setGripperVariable(CMD_ATR, 0);
+    waitForGripperStatus(GripperStatus::RESET);
+}
+
+
+void Robotiq2fSocketAdapter::deactivate() {
+    if (is_active()) {
+        reset();
+    }
+}
+
+// State subsscriber
+bool Robotiq2fSocketAdapter::is_active(){
+    GripperStatus status = static_cast<GripperStatus>(getGripperVariable(CMD_STA));
+    return GripperStatus::ACTIVE == status;
+}
+
+int Robotiq2fSocketAdapter::force(){
+    return getGripperVariable(CMD_FOR);
+}
+
+int Robotiq2fSocketAdapter::speed(){
+    return getGripperVariable(CMD_SPE);
+}
+
+int Robotiq2fSocketAdapter::position(){
+    return getGripperVariable(CMD_POS);
+}
+
+// Movement Methods
+std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
+    int clippedPosition = clip_val(_min_position, position, _max_position);
+    int clippedSpeed = clip_val(_min_speed, speed, _max_speed);
+    int clippedForce = clip_val(_min_force, force, _max_force);
+
+    // Create a map for gripper variables (similar to Python's OrderedDict)
+    std::map<std::string, int> variableMap = {
+        { CMD_POS, clippedPosition },
+        { CMD_SPE, clippedSpeed },
+        { CMD_FOR, clippedForce },
+        { CMD_GTO, 1 }
+    };
+
+    bool setResult = setGripperVariables(variableMap);
+    return std::make_tuple(setResult, clippedPosition);
+}
+
+std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
+    int clippedPosition = clip_val(min_position, position, max_position);
+    int clippedSpeed = clip_val(min_speed, speed, max_speed);
+    int clippedForce = clip_val(min_force, force, max_force);
+
+    // Set gripper variables to initiate the move
+    std::map<std::string, int> variableMap = {
+        { CMD_POS, clippedPosition },
+        { CMD_SPE, clippedSpeed },
+        { CMD_FOR, clippedForce },
+        { CMD_GTO, 1 }
+    };
+    bool setResult = setGripperVariables(variableMap);
+    if (!setResult) {
+        // Handle the error case, e.g., throw an exception
+        throw std::runtime_error("Failed to set variables for move.");
+    }
+
+    // Wait until position is acknowledged:
+    while(getGripperVariable(CMD_PRE) != clippedPosition) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
+    }
+
+    // Wait until not moving
+    int currentObjectStatus = getGripperVariable(CMD_OBJ);
+    while (ObjectStatus(currentObjectStatus) == ObjectStatus::MOVING) {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
+        currentObjectStatus = getGripperVariable(CMD_OBJ);
+    }
+
+    // Report final position and object status
+    int finalPosition = getGripperVariable(CMD_POS);
+    ObjectStatus finalStatus = static_cast<ObjectStatus>(currentObjectStatus);
+    return std::make_tuple(finalPosition, finalStatus);
+}
+
+
+void Robotiq2fSocketAdapter::auto_calibrate(bool log=true) {
+    // Open in case we are holding an object
+    std::tuple<int, ObjectStatus> result = move_and_wait_for_pos(open_position, 64, 1);
+    if (std::get<1>(result) != ObjectStatus::AT_DEST) {
+        throw std::runtime_error("Calibration failed opening to start.");
+    }
+
+    // Close as far as possible
+    result = move_and_wait_for_pos(closed_position, 64, 1);
+    if (std::get<1>(result) != ObjectStatus::AT_DEST) {
+        throw std::runtime_error("Calibration failed because of an object.");
+    }
+    _max_position = std::get<0>(result); // Update max position
+
+    // Open as far as possible
+    result = move_and_wait_for_pos(open_position, 64, 1);
+    if (std::get<1>(result) != ObjectStatus::AT_DEST) {
+        throw std::runtime_error("Calibration failed because of an object.");
+    }
+    _min_position = std::get<0>(result); // Update min position
+
+    if (log) {
+        std::cout << "Gripper auto-calibrated to ["
+                  << _min_position << ", " << _max_position << "]\n";
+    }
+}

src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp

@@ -0,0 +1,15 @@
+#include <hardware_interface.hpp>
+
+ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface()
+    : gripper_adapter_(std::make_unique<Robotiq2fSocketAdapter>())
+{
+
+}
+
+ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface() {
+    // 1. Disconnect from the gripper
+    gripper_adapter_->disconnect();
+
+    // 2. Clean up other resources if necessary
+    //    ...
+}

src/ur_robotiq_description/launch/tool_communication.launch.py

@@ -0,0 +1,67 @@
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterFile
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution
+
+
+def launch_setup(context, *args, **kwargs):
+    # Initialize Arguments
+    robot_ip = LaunchConfiguration("robot_ip")
+    tool_device_name = LaunchConfiguration("tool_device_name")
+    tool_tcp_port = LaunchConfiguration("tool_tcp_port")
+
+
+
+    tool_communication_node = Node(
+        package="ur_robot_driver",
+        executable="tool_communication.py",
+        name="ur_tool_comm",
+        output="screen",
+        parameters=[
+            {
+                "robot_ip": robot_ip,
+                "tcp_port": tool_tcp_port,
+                "device_name": tool_device_name,
+            }
+        ],
+    )
+
+
+
+    nodes_to_start = [tool_communication_node]
+
+    return nodes_to_start
+
+
+def generate_launch_description():
+    # Declare Arguments
+    declared_arguments = []
+    # UR specific arguments
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip", description="IP address by which the robot can be reached."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_device_name",
+            default_value="/tmp/ttyUR",
+            description="File descriptor that will be generated for the tool communication device. \
+            The user has be be allowed to write to this location. \
+            Only effective, if use_tool_communication is set to True.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "tool_tcp_port",
+            default_value="50006",
+            description="Remote port that will be used for bridging the tool's serial device. \
+            Only effective, if use_tool_communication is set to True.",
+        )
+    )
+
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

src/ur_robotiq_description/launch/ur_robotiq_control.launch.py

@@ -50,7 +50,7 @@ def launch_setup(context, *args, **kwargs):
         [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
     )
     kinematics_params = PathJoinSubstitution(
-        [FindPackageShare(ur_description_package), "config", ur_type, "default_kinematics.yaml"]
+        [FindPackageShare(description_package), "config","ur_robotiq_calibration.yaml"]
     )
     physical_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"]
@@ -348,18 +348,18 @@ def launch_setup(context, *args, **kwargs):
     )
 
     nodes_to_start = [
+                         # tool_communication_node,
                          control_node,
                          ur_control_node,
-                         robotiq_gripper_controller_spawner,
-                         robotiq_gripper_joint_trajectory_controller_spawner,
-                         robotiq_activation_controller_spawner,
                          dashboard_client_node,
-                         tool_communication_node,
                          controller_stopper_node,
                          robot_state_publisher_node,
                          rviz_node,
                          initial_joint_controller_spawner_stopped,
                          initial_joint_controller_spawner_started,
+                         robotiq_gripper_controller_spawner,
+                         robotiq_gripper_joint_trajectory_controller_spawner,
+                         robotiq_activation_controller_spawner,
                      ] + controller_spawners
 
     return nodes_to_start
@@ -505,7 +505,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_tool_communication",
-            default_value="false",
+            default_value="true",
             description="Only available for e series!",
         )
     )
@@ -561,7 +561,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "tool_tcp_port",
-            default_value="54321",
+            default_value="63352",
             description="Remote port that will be used for bridging the tool's serial device. \
             Only effective, if use_tool_communication is set to True.",
         )
@@ -569,7 +569,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "tool_voltage",
-            default_value="0",  # 0 being a conservative value that won't destroy anything
+            default_value="24",  # 0 being a conservative value that won't destroy anything
             description="Tool voltage that will be setup.",
         )
     )