finished implementation of new hardware interface

src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotiq_2f_controllers)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> 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_controllers/package.xml

@@ -0,0 +1,18 @@
+<?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_controllers</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_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_description/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotiq_2f_description)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> 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_description/config/robotiq_2f_140_config.yaml

@@ -0,0 +1,9 @@
+robotiq_2f_140_gripper:
+  ros__parameters:
+    # Physical limits
+    min_position: 0.0     # Meters (fully closed)
+    max_position: 0.14    # Meters (fully open)
+    min_speed: 0.02       # Meters per second
+    max_speed: 0.15       # Meters per second
+    min_force: 20.0       # Newtons
+    max_force: 235.0      # Newtons

src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_85_config.yaml

@@ -0,0 +1,11 @@
+robotiq_2f_140_gripper:
+  ros__parameters:
+    robot_ip: "192.168.1.11"
+    robot_port: 63352
+    # Physical limits
+    min_position: 0.0     # Meters (fully closed)
+    max_position: 0.085    # Meters (fully open)
+    min_speed: 0.02       # Meters per second
+    max_speed: 0.15       # Meters per second
+    min_force: 20.0       # Newtons
+    max_force: 235.0      # Newtons

src/robotiq_2f/robotiq_2f_description/package.xml

@@ -0,0 +1,18 @@
+<?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_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_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_description/urdf/robotiq_2f_140.ros2_control.xacro

@@ -0,0 +1,87 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="robotiq_gripper_ros2_control" params="
+        name
+        prefix
+        use_fake_hardware:=false
+        fake_sensor_commands:=false
+        robot_ip:=192.168.1.1
+        robot_port:=63352>
+
+        <ros2_control name="${name}" type="system">
+            <!-- Plugins -->
+            <hardware>
+                <xacro:if value="${use_fake_hardware}">
+                    <plugin>mock_components/GenericSystem</plugin>
+                    <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+                    <param name="state_following_offset">0.0</param>
+                </xacro:if>
+                <xacro:unless value="${use_fake_hardware}">
+                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
+                    <param name="gripper_closed_position">0.14</param>
+                    <param name="robot_ip">${robot_ip}</param>
+                    <param name="robot_port">${robot_port}</param>
+                    <param name="gripper_speed_multiplier">1.0</param>
+                    <param name="gripper_force_multiplier">0.5</param>
+                </xacro:unless>
+            </hardware>
+
+            <!-- Joint interfaces -->
+            <!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
+            <joint name="${prefix}finger_joint">
+                <command_interface name="position" />
+                <state_interface name="position">
+                    <param name="initial_value">0.14</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <!-- When simulating we need to include the rest of the gripper joints -->
+            <xacro:if value="${use_fake_hardware}">
+                <joint name="${prefix}left_inner_knuckle_joint">
+                    <param name="mimic">${prefix}finger_joint</param>
+                    <param name="multiplier">-1</param>
+                    <command_interface name="position"/>
+                    <state_interface name="position"/>
+                    <state_interface name="velocity"/>
+                </joint>
+                <joint name="${prefix}left_inner_finger_joint">
+                    <param name="mimic">${prefix}finger_joint</param>
+                    <param name="multiplier">1</param>
+                    <command_interface name="position"/>
+                    <state_interface name="position"/>
+                    <state_interface name="velocity"/>
+                </joint>
+                <joint name="${prefix}right_outer_knuckle_joint">
+                    <param name="mimic">${prefix}finger_joint</param>
+                    <param name="multiplier">-1</param>
+                    <command_interface name="position"/>
+                    <state_interface name="position"/>
+                    <state_interface name="velocity"/>
+                </joint>
+                <joint name="${prefix}robotiq_140_left_finger_tip_joint">
+                    <param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
+                    <param name="multiplier">-1</param>
+                    <command_interface name="position"/>
+                    <state_interface name="position"/>
+                    <state_interface name="velocity"/>
+                </joint>
+                <joint name="${prefix}robotiq_140_right_finger_tip_joint">
+                    <param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
+                    <param name="multiplier">1</param>
+                    <command_interface name="position"/>
+                    <state_interface name="position"/>
+                    <state_interface name="velocity"/>
+                </joint>
+            </xacro:if>
+
+            <!-- Only add this with fake hardware mode -->
+            <gpio name="reactivate_gripper">
+                <command_interface name="reactivate_gripper_cmd" />
+                <command_interface name="reactivate_gripper_response" />
+            </gpio>
+
+        </ros2_control>
+    </xacro:macro>
+
+</robot>

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

@@ -8,6 +8,7 @@
 #include <limits>
 #include <memory>
 #include <vector>
+#include <atomic>
 
 #include <Robotiq2fSocketAdapter.hpp>
 
@@ -17,46 +18,86 @@
 #include <rclcpp/rclcpp.hpp>
 #include <urdf/model.h>
 
+#include <stdexcept>
+
 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);
+    // Constructors
+    ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface();
+    ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface();
+    // ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr<SocketFactory> socket_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;
+    // 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;
+    // 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;
+    // 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_;
+    // 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();
+    // 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 ...
+    static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
+
+    // Conversion Methods
+    double convertRawToPosition(int raw_position);
+    int convertPositionToRaw(double position);
+    int convertSpeedToRaw(double speed);
+    int convertForceToRaw(double force);
+
+    // Gripper Commands
+    std::atomic<uint8_t> write_command_;
+    std::atomic<uint8_t> write_force_;
+    std::atomic<uint8_t> write_speed_;
+    std::atomic<uint8_t> gripper_current_state_;
+
+
+    // Gripper State Tracking
+    double gripper_position_ = 0.0;
+    double gripper_velocity_ = 0.0;
+    double gripper_position_command_ = 0.0;
+
+    double reactivate_gripper_cmd_ = 0.0;
+    std::atomic<bool> reactivate_gripper_async_cmd_;
+    double reactivate_gripper_response_ = 0.0;
+    double gripper_force_ = 0.0;
+    double gripper_speed_ = 0.0;
+    std::atomic<std::optional<bool>> reactivate_gripper_async_response_;
+
+    // Connection Variables
+    std::string robot_ip_;
+    int robot_port_;
+
+    // Parameters for physical limits and configuration
+    double min_position_;
+    double max_position_;
+    double min_speed_;
+    double max_speed_;
+    double min_force_;
+    double max_force_;
+
+    // loop time
+    constexpr auto gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
+
+    // Logger
+    const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
 };
 }
 

src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp

@@ -1,15 +1,385 @@
-#include <hardware_interface.hpp>
+#include "RobotiqGripperHardwareInterface.hpp"
 
-ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface()
-    : gripper_adapter_(std::make_unique<Robotiq2fSocketAdapter>())
+namespace robotiq_driver
 {
-
+RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
+  : communication_thread_is_running_(false)
+{
+  // socket_factory_ = std::make_unique<DefaultSocketFactory>();
 }
 
-ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface() {
-    // 1. Disconnect from the gripper
-    gripper_adapter_->disconnect();
+RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
+{
+  if (communication_thread_is_running_.load())
+  {
+    communication_thread_is_running_.store(false);
+    if (communication_thread_.joinable())
+    {
+      communication_thread_.join();
+    }
+  }
+}
 
-    // 2. Clean up other resources if necessary
-    //    ...
-}
+hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
+{
+    RCLCPP_DEBUG(rclcpp::get_logger("RobotiqGripperHardwareInterface"), "on_init");
+
+    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+    {
+        return CallbackReturn::ERROR;
+    }
+
+    // Loading paramters from Hardware info and .yaml file
+       auto node = this->get_node();
+    node->declare_parameter("min_position", 0.0);
+    node->declare_parameter("max_position", 0.14);
+    node->declare_parameter("min_speed", 0.02);
+    node->declare_parameter("max_speed", 0.15);
+    node->declare_parameter("min_force", 20.0);
+    node->declare_parameter("max_force", 235.0);
+    try
+    {
+        robot_ip_ =  info_.hardware_parameters.at("robot_ip");
+        robot_port_ =  std::stod(info_.hardware_parameters.at("robot_port"));
+        node->get_parameter("min_position", min_position_);
+        node->get_parameter("max_position", max_position_);
+        node->get_parameter("min_speed", min_speed_);
+        node->get_parameter("max_speed", max_speed_);
+        node->get_parameter("min_force", min_force_);
+        node->get_parameter("max_force", max_force_);
+    }
+    catch (const std::exception& e)
+    {
+        RCLCPP_FATAL(rclcpp::get_logger("RobotiqGripperHardwareInterface"),
+                     "Failed to load parameters: %s", e.what());
+        return CallbackReturn::ERROR;
+    }
+
+    gripper_position_ = std::numeric_limits<double>::quiet_NaN();
+    gripper_velocity_ = std::numeric_limits<double>::quiet_NaN();
+    gripper_position_command_ = std::numeric_limits<double>::quiet_NaN();
+    reactivate_gripper_cmd_ = NO_NEW_CMD_;
+    reactivate_gripper_async_cmd_.store(false);
+
+    const hardware_interface::ComponentInfo& joint = info_.joints[0];
+
+    // There is one command interface: position.
+    if (joint.command_interfaces.size() != 1)
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
+                     joint.command_interfaces.size());
+        return CallbackReturn::ERROR;
+    }
+
+    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(),
+                     joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+        return CallbackReturn::ERROR;
+    }
+
+    // There are two state interfaces: position and velocity.
+    if (joint.state_interfaces.size() != 2)
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
+                     joint.state_interfaces.size());
+        return CallbackReturn::ERROR;
+    }
+
+    for (int i = 0; i < 2; ++i)
+    {
+        if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
+              joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
+        {
+            RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(),
+                       joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
+                       hardware_interface::HW_IF_VELOCITY);
+            return CallbackReturn::ERROR;
+        }
+    }
+
+    // Socket Factory -> not added yet
+
+    return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+    RCLCPP_DEBUG(logger_, "on_configure");
+    try
+    {
+        if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS)
+        {
+            return CallbackReturn::ERROR;
+        }
+
+        socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
+        if (!socket_adapter_->connect(robot_ip_, robot_port_))
+        {
+            RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
+            return CallbackReturn::ERROR;
+        }
+    }
+    catch (const std::exception& e)
+    {
+        RCLCPP_ERROR(logger_, "Cannot configure the Robotiq gripper: %s", e.what());
+        return CallbackReturn::ERROR;
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+    RCLCPP_DEBUG(logger_, "on_activate");
+
+    // set some default values for joints
+    if (std::isnan(gripper_position_))
+    {
+        gripper_position_ = 0;
+        gripper_velocity_ = 0;
+        gripper_position_command_ = 0;
+    }
+
+    // Activate the gripper.
+    try
+    {
+        socket_adapter_->deactivate();
+        socket_adapter_->activate();
+
+        communication_thread_is_running_.store(true);
+        communication_thread_ = std::thread([this] { this->background_task(); });
+    }
+    catch (const std::exception& e)
+    {
+        RCLCPP_FATAL(logger_, "Failed to communicate with the Robotiq gripper: %s", e.what());
+        return CallbackReturn::ERROR;
+    }
+
+    RCLCPP_INFO(logger_, "Robotiq Gripper successfully activated!");
+    return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+    RCLCPP_DEBUG(logger_, "on_deactivate");
+
+    communication_thread_is_running_.store(false);
+    communication_thread_.join();
+    if (communication_thread_.joinable())
+    {
+        communication_thread_.join();
+    }
+
+    try
+    {
+        socket_adapter_->deactivate();
+    }
+    catch (const std::exception& e)
+    {
+        RCLCPP_ERROR(logger_, "Failed to deactivate the Robotiq gripper: %s", e.what());
+        return CallbackReturn::ERROR;
+    }
+    RCLCPP_INFO(logger_, "Robotiq Gripper successfully deactivated!");
+    return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces()
+{
+    RCLCPP_DEBUG(logger_, "export_state_interfaces");
+
+    std::vector<hardware_interface::StateInterface> state_interfaces;
+
+    state_interfaces.emplace_back(
+        hardware_interface::StateInterface(
+            info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_
+        )
+    );
+
+    state_interfaces.emplace_back(
+        hardware_interface::StateInterface(
+            info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_
+        )
+    );
+
+    return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterface::export_command_interfaces()
+{
+    RCLCPP_DEBUG(logger_, "export_command_interfaces");
+
+    std::vector<hardware_interface::CommandInterface> command_interfaces;
+
+    command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(
+        info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_
+        )
+    );
+
+    // Handling potential uninitialized parameter 'gripper_speed_multiplier'
+    double gripper_speed_multiplier = 1.0;
+    if (info_.hardware_parameters.find("gripper_speed_multiplier") != info_.hardware_parameters.end()) {
+        gripper_speed_multiplier = std::stod(info_.hardware_parameters["gripper_speed_multiplier"]);
+    }
+    gripper_speed_ = gripper_speed_multiplier;
+
+    // Handling potential uninitialized parameter 'gripper_force_multiplier'
+    double gripper_force_multiplier = 1.0;
+    if (info_.hardware_parameters.find("gripper_force_multiplier") != info_.hardware_parameters.end()) {
+        gripper_force_multiplier = std::stod(info_.hardware_parameters["gripper_force_multiplier"]);
+    }
+    gripper_force_ = gripper_force_multiplier;
+
+    command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_));
+    command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_));
+    command_interfaces.emplace_back(
+        hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
+    command_interfaces.emplace_back(
+        hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_));
+
+    return command_interfaces;
+}
+
+hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& time, const rclcpp::Duration& /*period*/)
+{
+    RCLCPP_DEBUG(logger_, "Reading state from the gripper");
+
+    // Fetch current position and other states from the hardware
+    try {
+        int raw_position = socket_adapter_->getGripperPosition();  // This should be an existing method in your adapter
+        double new_position = convertRawToPosition(raw_position);
+        rclcpp::Time current_time = time;
+
+        if (!std::isnan(gripper_position_)) {
+            double time_difference = (current_time - last_update_time_).seconds();  // Calculate time difference in seconds
+            if (time_difference > 0) {
+                gripper_velocity_ = (new_position - gripper_position_) / time_difference;  // Calculate velocity
+            }
+        }
+
+        gripper_position_ = new_position;  // Update current position
+        last_update_time_ = current_time;  // Update last update timestamp
+
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(logger_, "Failed to read from Robotiq gripper: %s", e.what());
+        return hardware_interface::return_type::ERROR;
+    }
+
+    // Handle reactivation command if set
+    if (!std::isnan(reactivate_gripper_cmd_) && reactivate_gripper_cmd_ != NO_NEW_CMD_) {
+        RCLCPP_INFO(logger_, "Sending gripper reactivation request.");
+        reactivate_gripper_async_cmd_.store(true);
+        reactivate_gripper_cmd_ = NO_NEW_CMD_;  // Reset command
+    }
+
+    // Process asynchronous reactivation response if available
+    if (reactivate_gripper_async_response_.load().has_value()) {
+        reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value();
+        reactivate_gripper_async_response_.store(std::nullopt);  // Reset response
+    }
+
+    return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+    try {
+        // Use the conversion methods to scale the command values to the range expected by the hardware
+        int scaled_position = convertPositionToRaw(gripper_position_command_);
+        int scaled_speed = convertSpeedToRaw(gripper_speed_);
+        int scaled_force = convertForceToRaw(gripper_force_);
+
+        // Store the scaled values into atomic variables
+        write_command_.store(scaled_position);
+        write_speed_.store(scaled_speed);
+        write_force_.store(scaled_force);
+
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(logger_, "Failed to write to Robotiq gripper: %s", e.what());
+        return hardware_interface::return_type::ERROR;
+    }
+
+    return hardware_interface::return_type::OK;
+}
+
+void RobotiqGripperHardwareInterface::background_task()
+{
+  while (communication_thread_is_running_.load())
+  {
+    try
+    {
+      // Re-activate the gripper
+      // This can be used, for example, to re-run the auto-calibration.
+      if (reactivate_gripper_async_cmd_.load())
+      {
+        socket_adapter_->deactivate();
+        socket_adapter_->activate();
+        reactivate_gripper_async_cmd_.store(false);
+        reactivate_gripper_async_response_.store(true);
+      }
+
+      // Write the latest command to the gripper.
+      int scaled_position = write_command_.load();
+      int scaled_speed = write_speed_.load();
+      int scaled_force = write_force_.load();
+
+      if (!socket_adapter_->move(scaled_position, scaled_speed, scaled_force)) {
+        throw std::runtime_error("Failed to send move command to Robotiq gripper.");
+      }
+
+      // Read the state of the gripper.
+      int raw_position = socket_adapter_->getGripperPosition();
+      gripper_current_state_.store(convertRawToPosition(raw_position));
+    }
+    catch (std::exception& e)
+    {
+      RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
+    }
+
+    std::this_thread::sleep_for(gripperCommsLoopPeriod);  // Sleep for a predefined period
+  }
+}
+
+
+
+// Conversion methods
+double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
+    if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
+        throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
+    }
+    double scaled_position = min_position_ + (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
+    return std::max(min_position_, std::min(scaled_position, max_position_));
+}
+
+uint8_t RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
+    if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
+        throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
+    }
+    double scaled = (position - min_position_) / (max_position_ - min_position_) * 255.0;
+    return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
+}
+
+uint8_t RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
+    if (std::isnan(min_speed_) || std::isnan(max_speed_) || min_speed_ >= max_speed_) {
+        throw std::runtime_error("Invalid gripper speed limits: min_speed_ or max_speed_ are not configured correctly.");
+    }
+    double scaled = (speed - min_speed_) / (max_speed_ - min_speed_) * 255.0;
+    return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
+}
+
+uint8_t RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
+    if (std::isnan(min_force_) || std::isnan(max_force_) || min_force_ >= max_force_) {
+        throw std::runtime_error("Invalid gripper force limits: min_force_ or max_force_ are not configured correctly.");
+    }
+    double scaled = (force - min_force_) / (max_force_ - min_force_) * 255.0;
+    return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
+}
+
+}  // namespace robotiq_driver
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(robotiq_2f_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)

src/robotiq_2f/robotiq_2f_interface/tests/test_conversions.cpp

@@ -0,0 +1,69 @@
+#include <gtest/gtest.h>
+#include "RobotiqGripperHardwareInterface.hpp"
+
+class TestableRobotiqGripperHardwareInterface : public robotiq_driver::RobotiqGripperHardwareInterface {
+public:
+    using robotiq_driver::RobotiqGripperHardwareInterface::convertPositionToRaw;
+    using robotiq_driver::RobotiqGripperHardwareInterface::convertRawToPosition;
+    using robotiq_driver::RobotiqGripperHardwareInterface::convertSpeedToRaw;
+    using robotiq_driver::RobotiqGripperHardwareInterface::convertForceToRaw;
+};
+
+class RobotiqGripperConversionTests : public ::testing::Test {
+protected:
+    TestableRobotiqGripperHardwareInterface gripper_interface;
+
+    void SetUp() override {
+        gripper_interface.min_position_ = 0.0;
+        gripper_interface.max_position_ = 0.14;
+        gripper_interface.min_speed_ = 0.02;
+        gripper_interface.max_speed_ = 0.15;
+        gripper_interface.min_force_ = 20.0;
+        gripper_interface.max_force_ = 235.0;
+    }
+};
+
+// Test for convertPositionToRaw
+TEST_F(RobotiqGripperConversionTests, PositionToRawValidInput) {
+    EXPECT_EQ(gripper_interface.convertPositionToRaw(0.07), 128);
+}
+
+TEST_F(RobotiqGripperConversionTests, PositionToRawBoundaryInput) {
+    EXPECT_EQ(gripper_interface.convertPositionToRaw(0.0), 0);
+    EXPECT_EQ(gripper_interface.convertPositionToRaw(0.14), 255);
+}
+
+// Test for convertRawToPosition
+TEST_F(RobotiqGripperConversionTests, RawToPositionValidInput) {
+    ASSERT_NEAR(gripper_interface.convertRawToPosition(128), 0.07, 0.01);
+}
+
+TEST_F(RobotiqGripperConversionTests, RawToPositionBoundaryInput) {
+    ASSERT_NEAR(gripper_interface.convertRawToPosition(0), 0.0, 0.01);
+    ASSERT_NEAR(gripper_interface.convertRawToPosition(255), 0.14, 0.01);
+}
+
+// Test for convertSpeedToRaw
+TEST_F(RobotiqGripperConversionTests, SpeedToRawValidInput) {
+    EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.085), 128);
+}
+
+TEST_F(RobotiqGripperConversionTests, SpeedToRawBoundaryInput) {
+    EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.02), 0);
+    EXPECT_EQ(gripper_interface.convertSpeedToRaw(0.15), 255);
+}
+
+// Test for convertForceToRaw
+TEST_F(RobotiqGripperConversionTests, ForceToRawValidInput) {
+    EXPECT_EQ(gripper_interface.convertForceToRaw(127.5), 128);
+}
+
+TEST_F(RobotiqGripperConversionTests, ForceToRawBoundaryInput) {
+    EXPECT_EQ(gripper_interface.convertForceToRaw(20.0), 0);
+    EXPECT_EQ(gripper_interface.convertForceToRaw(235.0), 255);
+}
+
+int main(int argc, char **argv) {
+    ::testing::InitGoogleTest(&argc, argv);
+    return RUN_ALL_TESTS();
+}