smooth movement of the gripper

Added mapping routine for ps5 controller

.idea/.gitignore

@@ -6,5 +6,3 @@
 # Datasource local storage ignored files
 /dataSources/
 /dataSources.local.xml
-# GitHub Copilot persisted chat sessions
-/copilot/chatSessions

.idea/vcs.xml

@@ -4,8 +4,5 @@
     <mapping directory="$PROJECT_DIR$" vcs="Git" />
     <mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Description" vcs="Git" />
     <mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Driver" vcs="Git" />
-    <mapping directory="$PROJECT_DIR$/src/cartesian_controllers" vcs="Git" />
-    <mapping directory="$PROJECT_DIR$/src/ros2_robotiq_gripper" vcs="Git" />
-    <mapping directory="$PROJECT_DIR$/src/serial" vcs="Git" />
   </component>
 </project>

README.md

@@ -78,42 +78,17 @@ ros2 launch ur_robotiq_moveit_config ur_robotiq_moveit.launch.py use_fake_hardwa
 ```
 
 ### servoing
-#### PS5 controller
 Terminal 3:\
 **At the moment the servoing doesnt work with the collision checker (self collision still works).**
 ```bash
 ros2 launch ur_robotiq_servo ps5_servo.launch.py
 ```
-#### Keyboard
-Terminal 3:
-```bash
-ros2 run servo_keyboard servo_keyboard_input --ros-args --params-file src/servo_keyboard/config/servo_keyboard_params.yaml
-```
 
 After launching you need to change the controller to enable the servo mode.\
 Terminal 4:
 ```bash
 ros2 control switch_controllers --deactivate joint_trajectory_controller
-ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
+ros2 control switch_controllers --deactivate robotiq_gripper_controller
 ros2 control switch_controllers --activate forward_position_controller
 ros2 control switch_controllers --activate forward_gripper_position_controller
-```
-
-For the real robot you need to change to the scaled position controller:
-```bash
-ros2 control switch_controllers --deactivate scaled_joint_trajectory_controller
-ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
-ros2 control switch_controllers --activate scaled_forward_position_controller
-ros2 control switch_controllers --activate forward_gripper_position_controller
-```
-
-Terminal 5:
-Activate the Servo Node
-```bash
-ros2 service call /servo_node/start_servo std_srvs/srv/Trigger {}
-```
-
-### extract the urdf from xacro file
-```bash
-ros2 run xacro xacro src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro > src/ur_robotiq_description/urdf/ur_robotiq.urdf name:=ur3e
 ```

UR_Robotiq.humble.repos

@@ -7,14 +7,7 @@ repositories:
     type: git
     url: https://github.com/NikoFeith/Universal_Robots_ROS2_Driver.git
     version: humble
-  serial:
-    type: git
-    url: https://github.com/tylerjw/serial.git
-    version: ros2
-  ros2_robotiq_gripper:
-    type: git
-    url: https://github.com/PickNikRobotics/ros2_robotiq_gripper.git
-    version: main
   cartesian_controllers:
     type: git
-    url: https://github.com/NikoFeith/cartesian_controllers.git
+    url: https://github.com/NikoFeith/cartesian_controllers.git
+    version: ros2

src/Universal_Robots_ROS2_Description

@@ -0,0 +1 @@
+Subproject commit f5394b8e7439632d26719cd7b47ee9102a921078

src/Universal_Robots_ROS2_Driver

@@ -0,0 +1 @@
+Subproject commit ca5422994c410b3b8c512100144e758e1499705a

src/robotiq_2f/README.md

@@ -0,0 +1,60 @@
+**robotiq_2f Repository** 
+
+***Overview***
+
+This repository provides a ROS2 integration for controlling the Robotiq 2F-85 and 2F-140 grippers. It combines elements from:
+
+* **ros2_robotiq_gripper:** [https://github.com/PickNikRobotics/ros2_robotiq_gripper/tree/main](https://github.com/PickNikRobotics/ros2_robotiq_gripper/tree/main)
+* **robotiq_2f_urcap_adapter:** [https://github.com/fzi-forschungszentrum-informatik/robotiq_2f_urcap_adapter/tree/main](https://github.com/fzi-forschungszentrum-informatik/robotiq_2f_urcap_adapter/tree/main)
+
+It leverages the low-level socket communication provided by the robotiq_2f_urcap_adapter to create a new ROS2-compatible hardware interface.
+
+***Features***
+
+* **robotiq_2f_controllers:**
+    * ForwardController
+    * GripperCommand
+    * RobotiqActivationController (inherited by ros2_robotiq_gripper) 
+* **robotiq_2f_description:**
+    * Configuration files for 85mm and 140mm grippers
+    * Controller configuration files
+    * Launch files for RViz visualization and controller activation
+    * Meshes for visualization and collision detection
+    * Modified URDF files
+* **robotiq_2f_interface:**
+    * ROS2 hardware interface 
+    * Robotiq2fSocketAdapter 
+    * MockRobotiq2fSocketAdapter
+* **robotiq_2f_msgs:**
+    * GripperCommand.action
+    * ForwardCommand.msg
+
+***Socket Communication***
+
+Socket communication details: [https://dof.robotiq.com/discussion/2420/control-robotiq-gripper-mounted-on-ur-robot-via-socket-communication-python](https://dof.robotiq.com/discussion/2420/control-robotiq-gripper-mounted-on-ur-robot-via-socket-communication-python)
+
+***Robotiq Resources***
+
+* **Robotiq UR Cap:** [https://robotiq.com/products/2f85-140-adaptive-robot-gripper?ref=nav_product_new_button](https://robotiq.com/products/2f85-140-adaptive-robot-gripper?ref=nav_product_new_button)
+* **Quick Start Guide:** [https://blog.robotiq.com/hubfs/Support%20Documents/QSG/Quick_start_2Finger_e-Series_nocropmarks_EN.pdf](https://blog.robotiq.com/hubfs/Support%20Documents/QSG/Quick_start_2Finger_e-Series_nocropmarks_EN.pdf)
+
+***Parameters***
+
+* `robot_ip`: IP address of the robot controller
+* `robot_port`: Port for robot controller communication
+* `use_fake_hardware`: Start gripper with fake hardware mirroring command to its states.
+* `use_forward_controller`: Use forward controller instead of gripper command controller(action controller).
+* `prefix`: In case multiple grippers are used on the same UR Cap.
+* `description_file`: The type of the Gripper (85/140) is decided here.
+
+**robotiq_2f_gripper (config file)**
+
+* `min_position`: 0.0 (fully closed)
+* `max_position`: 0.14 (fully open)
+* `max_angle`: 0.695 (radians, closed)
+* `min_speed`: 0.02 
+* `max_speed`: 0.15 
+* `min_force`: 20.0 
+* `max_force`: 235.0 
+
+

src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt

@@ -0,0 +1,53 @@
+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(rclcpp REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(robotiq_2f_msgs REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(std_srvs REQUIRED)
+
+include_directories(include)
+
+add_library(robotiq_2f_controllers SHARED
+  src/robotiq_forward_controller.cpp
+  src/robotiq_gripper_command_controller.cpp
+  src/robotiq_activation_controller.cpp
+)
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  controller_interface
+  hardware_interface
+  pluginlib
+  robotiq_2f_msgs
+  realtime_tools
+  std_srvs
+)
+pluginlib_export_plugin_description_file(controller_interface controller_plugin.xml)
+
+
+# Install targets
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# Install header files
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+# Install plugin description file
+install(FILES
+  controller_plugin.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+ament_package()

src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml

@@ -0,0 +1,17 @@
+<library path="robotiq_2f_controllers">
+    <class name="robotiq_2f_controllers/ForwardController" type="robotiq_2f_controllers::RobotiqForwardController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            Forward controller for Robotiq 2F Gripper.
+        </description>
+    </class>
+        <class name="robotiq_2f_controllers/GripperCommand" type="robotiq_2f_controllers::RobotiqGripperCommandController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            Gripper Command controller for Robotiq 2F Gripper.
+        </description>
+    </class>
+    <class name="robotiq_2f_controllers/RobotiqActivationController" type="robotiq_controllers::RobotiqActivationController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            This controller provides an interface to (re-)activate the Robotiq gripper.
+        </description>
+    </class>
+</library>

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_activation_controller.hpp

@@ -0,0 +1,64 @@
+// Copyright (c) 2022 PickNik, Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include "controller_interface/controller_interface.hpp"
+#include "std_srvs/srv/trigger.hpp"
+
+namespace robotiq_controllers
+{
+class RobotiqActivationController : public controller_interface::ControllerInterface
+{
+public:
+  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  CallbackReturn on_init() override;
+
+private:
+  bool reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr req,
+                         std_srvs::srv::Trigger::Response::SharedPtr resp);
+
+  static constexpr double ASYNC_WAITING = 2.0;
+  enum CommandInterfaces
+  {
+    REACTIVATE_GRIPPER_CMD,
+    REACTIVATE_GRIPPER_RESPONSE
+  };
+
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reactivate_gripper_srv_;
+};
+}  // namespace robotiq_controllers

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp

@@ -0,0 +1,100 @@
+
+
+#ifndef ROBOTIQ_FORWARD_CONTROLLER_HPP
+#define ROBOTIQ_FORWARD_CONTROLLER_HPP
+
+#include <stddef.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include "rclcpp/qos.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
+#include "realtime_tools/realtime_buffer.h"
+
+#include "robotiq_2f_msgs/msg/forward_command.hpp"
+
+namespace robotiq_2f_controllers
+{
+class RobotiqForwardController : public controller_interface::ControllerInterface
+{
+public:
+    CONTROLLER_INTERFACE_PUBLIC
+    RobotiqForwardController();
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_init() override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+    std::string joint_name_;
+    std::vector<std::string> command_interface_types_;
+    std::vector<std::string> state_interface_types_;
+
+    // communication interface
+    bool new_msg_ = false;
+    rclcpp::Subscription<robotiq_2f_msgs::msg::ForwardCommand>::SharedPtr command_subscriber_;
+    std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> command_msg_;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand>>
+    msg_external_ptr_;
+
+    // Command Interfaces
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    joint_position_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_velocity_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_effort_command_interface_;
+
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
+    command_interface_map_ = {
+      {"position", &joint_position_command_interface_},
+      {"max_velocity", &max_velocity_command_interface_},
+      {"max_effort", &max_effort_command_interface_}};
+
+    double desired_position_;
+    double desired_max_velocity_;
+    double desired_max_effort_;
+
+};
+} // end namespace robotiq_2f_controllers
+#endif

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp

@@ -0,0 +1,143 @@
+
+
+#ifndef ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP
+#define ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP
+
+#include <stddef.h>
+#include <memory>
+#include <string>
+#include <vector>
+#include <queue>
+#include <mutex>
+
+#include <stdexcept> // For std::runtime_error
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/qos.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp/callback_group.hpp>
+
+#include <controller_interface/controller_interface.hpp>
+#include <hardware_interface/loaned_command_interface.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
+#include "realtime_tools/realtime_buffer.h"
+
+#include "robotiq_2f_msgs/action/gripper_command.hpp"
+
+using GripperGoalHandle = rclcpp_action::ServerGoalHandle<robotiq_2f_msgs::action::GripperCommand>;
+
+namespace robotiq_2f_controllers
+{
+class RobotiqGripperCommandController : public controller_interface::ControllerInterface
+{
+public:
+    CONTROLLER_INTERFACE_PUBLIC
+    RobotiqGripperCommandController();
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_init() override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+    std::mutex mutex_;
+    std::string joint_name_;
+    std::vector<std::string> command_interface_types_;
+    std::vector<std::string> state_interface_types_;
+
+    // Checker Methods
+    bool command_interface_checker_();
+    bool is_valid_goal(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
+    bool is_stalled(double current_position);
+    bool is_stopped();
+
+    // Action Interface
+    std::queue<std::shared_ptr<GripperGoalHandle>> goal_queue_;
+    std::shared_ptr<GripperGoalHandle> current_goal_handle_;
+    rclcpp_action::Server<robotiq_2f_msgs::action::GripperCommand>::SharedPtr action_server_;
+
+    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & goal_uuid,
+         std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
+    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GripperGoalHandle> goal_handle);
+    void handle_accepted(const std::shared_ptr<GripperGoalHandle> goal_handle);
+    void process_next_goal();
+    void execute_goal(const std::shared_ptr<GripperGoalHandle> goal_handle);
+
+    void publish_feedback();
+
+    // Error Related
+    bool stop_motion(double current_position);
+
+    // Command Interfaces
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    joint_position_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_velocity_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_effort_command_interface_;
+
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
+    command_interface_map_ = {
+      {"position", &joint_position_command_interface_},
+      {"max_velocity", &max_velocity_command_interface_},
+      {"max_effort", &max_effort_command_interface_}};
+
+    // State Interfaces
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+    joint_position_state_interface_;
+
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> *>
+    state_interface_map_ = {
+      {"position", &joint_position_state_interface_}};
+
+    // Addtional State variables
+    double max_position_ = 0.0;
+    double target_position_ = -1.0;
+    double current_position_ = -1.0;
+    double last_position_ = -1.0;
+    int stall_counter_ = 0;
+    bool cancel_requested_ = false;
+
+    // Const
+    const int MAX_STALL_COUNT = 200;
+    const double POSITION_TOLERANCE = 0.0015;
+    const double MIN_SPEED = 0.0;
+    const double MIN_EFFORT = 0.0;
+
+};
+} // end namespace robotiq_2f_controllers
+#endif

src/robotiq_2f/robotiq_2f_controllers/package.xml

@@ -0,0 +1,26 @@
+<?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>
+
+  <depend>rclcpp</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>robotiq_2f_msgs</depend>
+  <depend>realtime_tools</depend>
+  <depend>std_srvs</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_controllers/src/robotiq_activation_controller.cpp

@@ -0,0 +1,123 @@
+// Copyright (c) 2022 PickNik, Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include "robotiq_2f_controllers/robotiq_activation_controller.hpp"
+
+namespace robotiq_controllers
+{
+controller_interface::InterfaceConfiguration RobotiqActivationController::command_interface_configuration() const
+{
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  config.names.emplace_back("reactivate_gripper/reactivate_gripper_cmd");
+  config.names.emplace_back("reactivate_gripper/reactivate_gripper_response");
+
+  return config;
+}
+
+controller_interface::InterfaceConfiguration RobotiqActivationController::state_interface_configuration() const
+{
+  controller_interface::InterfaceConfiguration config;
+  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+  return config;
+}
+
+controller_interface::return_type RobotiqActivationController::update(const rclcpp::Time& /*time*/,
+                                                                      const rclcpp::Duration& /*period*/)
+{
+  return controller_interface::return_type::OK;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+RobotiqActivationController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  // Check command interfaces.
+  if (command_interfaces_.size() != 2)
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "Expected %d command interfaces, but got %zu.", 2,
+                 command_interfaces_.size());
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+
+  try
+  {
+    // Create service for re-activating the gripper.
+    reactivate_gripper_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
+        "~/reactivate_gripper",
+        [this](std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp) {
+          this->reactivateGripper(req, resp);
+        });
+  }
+  catch (...)
+  {
+    return LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+  return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+RobotiqActivationController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
+{
+  try
+  {
+    reactivate_gripper_srv_.reset();
+  }
+  catch (...)
+  {
+    return LifecycleNodeInterface::CallbackReturn::ERROR;
+  }
+
+  return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqActivationController::on_init()
+{
+  return LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+bool RobotiqActivationController::reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
+                                                    std_srvs::srv::Trigger::Response::SharedPtr resp)
+{
+  command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING);
+  command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0);
+
+  while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING)
+  {
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  }
+  resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value();
+
+  return resp->success;
+}
+}  // namespace robotiq_controllers
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(robotiq_controllers::RobotiqActivationController, controller_interface::ControllerInterface)

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp

@@ -0,0 +1,137 @@
+#include "robotiq_2f_controllers/robotiq_forward_controller.hpp"
+
+
+using config_type = controller_interface::interface_configuration_type;
+
+namespace robotiq_2f_controllers
+{
+RobotiqForwardController::RobotiqForwardController() : controller_interface::ControllerInterface() {}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_init()
+{
+    joint_name_ = auto_declare<std::string>("joint", joint_name_);
+    command_interface_types_ =
+    auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+    state_interface_types_ =
+    auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
+
+
+    if (joint_name_.empty()) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint_name' parameter.");
+        return CallbackReturn::ERROR;
+    }
+    else{
+        RCLCPP_DEBUG(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str());
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces.");
+
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto& interface_map_entry : command_interface_map_) {
+        const std::string& interface_type = interface_map_entry.first;
+        config.names.push_back(joint_name_ + "/" + interface_type);
+    }
+    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces.");
+
+    return config;
+}
+
+
+controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const {
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::NONE;
+    return config;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State &)
+{
+    auto callback =
+        [this](const std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> cmd_msg) -> void
+        {
+            msg_external_ptr_.writeFromNonRT(cmd_msg);
+            new_msg_ = true;
+        };
+
+    command_subscriber_ = get_node()->create_subscription<robotiq_2f_msgs::msg::ForwardCommand>(
+        "~/command", rclcpp::SystemDefaultsQoS(), callback);
+
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State &)
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Available command interfaces:");
+    for (const auto& interface : command_interfaces_) {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str());
+    }
+
+    // clear out vectors in case of restart
+    joint_position_command_interface_.clear();
+    max_velocity_command_interface_.clear();
+    max_effort_command_interface_.clear();
+
+    // assign command interfaces
+    for (auto & interface : command_interfaces_)
+    {
+        command_interface_map_[interface.get_interface_name()]->push_back(interface);
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+    if (new_msg_)
+    {
+        command_msg_ = *msg_external_ptr_.readFromRT();
+        desired_position_ = command_msg_->position;
+        desired_max_velocity_ = command_msg_->max_velocity;
+        desired_max_effort_ = command_msg_->max_effort;
+    }
+    // Set the hardware interface commands
+    if (joint_position_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Command interface is not available.");
+        return controller_interface::return_type::ERROR;
+    }
+
+    joint_position_command_interface_[0].get().set_value(desired_position_);
+    max_velocity_command_interface_[0].get().set_value(desired_max_velocity_);
+    max_effort_command_interface_[0].get().set_value(desired_max_effort_);
+
+    return controller_interface::return_type::OK;
+}
+
+
+controller_interface::CallbackReturn RobotiqForwardController::on_deactivate(const rclcpp_lifecycle::State &)
+{
+  release_interfaces();
+
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_cleanup(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_error(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+}
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface)
+

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp

@@ -0,0 +1,389 @@
+#include "robotiq_2f_controllers/robotiq_gripper_command_controller.hpp"
+
+
+using config_type = controller_interface::interface_configuration_type;
+
+namespace robotiq_2f_controllers
+{
+RobotiqGripperCommandController::RobotiqGripperCommandController() : controller_interface::ControllerInterface() {}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_init()
+{
+    joint_name_ = auto_declare<std::string>("joint", joint_name_);
+    max_position_ = auto_declare<double>("max_gap", max_position_);
+    command_interface_types_ =
+    auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+    state_interface_types_ =
+    auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
+
+
+    if (joint_name_.empty()) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint' parameter.");
+        return CallbackReturn::ERROR;
+    }
+    else{
+        RCLCPP_DEBUG(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str());
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration RobotiqGripperCommandController::command_interface_configuration() const
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces.");
+
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto& interface_map_entry : command_interface_map_) {
+        const std::string& interface_type = interface_map_entry.first;
+        config.names.push_back(joint_name_ + "/" + interface_type);
+    }
+    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces.");
+    return config;
+}
+
+
+controller_interface::InterfaceConfiguration RobotiqGripperCommandController::state_interface_configuration() const
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of state interfaces.");
+
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto& interface_map_entry : state_interface_map_) {
+        const std::string& interface_type = interface_map_entry.first;
+        config.names.push_back(joint_name_ + "/" + interface_type);
+    }
+
+    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of state interfaces.");
+    return config;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_configure(const rclcpp_lifecycle::State &)
+{
+    action_server_ = rclcpp_action::create_server<robotiq_2f_msgs::action::GripperCommand>(
+        get_node(),
+        "~/gripper_command",
+        [this](const std::array<unsigned char, 16>& uuid, std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal) {
+            return this->handle_goal(uuid, goal);
+        },
+        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {return this->handle_cancel(goal_handle);},
+        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {this->handle_accepted(goal_handle);}
+    );
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_activate(const rclcpp_lifecycle::State & )
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Available command interfaces:");
+    for (const auto& interface : command_interfaces_) {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str());
+    }
+
+    // clear out vectors in case of restart
+    joint_position_command_interface_.clear();
+    max_velocity_command_interface_.clear();
+    max_effort_command_interface_.clear();
+    joint_position_state_interface_.clear();
+
+    // assign command interfaces
+    for (auto & interface : command_interfaces_)
+    {
+        command_interface_map_[interface.get_interface_name()]->push_back(interface);
+    }
+
+    // assign state interface
+        for (auto & interface : state_interfaces_)
+    {
+        state_interface_map_[interface.get_interface_name()]->push_back(interface);
+    }
+
+    RCLCPP_DEBUG(get_node()->get_logger(), "Checking if command interfaces are initialized corretly...");
+    if (!command_interface_checker_())
+    {
+        return CallbackReturn::ERROR;
+    }
+    RCLCPP_DEBUG(get_node()->get_logger(), "Command interfaces correctly initialized!");
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type RobotiqGripperCommandController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+    // Check if there is an active goal
+    if (current_goal_handle_) {
+
+        // Retrieve the current position of the gripper
+        try
+        {
+            if (joint_position_state_interface_.empty())
+            {
+                throw std::runtime_error("State interface is not correctly initialized");
+            }
+
+            current_position_ = joint_position_state_interface_[0].get().get_value();
+        } catch (const std::exception& e) {
+            RCLCPP_FATAL(get_node()->get_logger(), "Failed to access state interface: %s", e.what());
+            return controller_interface::return_type::ERROR;
+        }
+
+
+        // Check if the gripper is stalled
+        if (is_stalled(current_position_)) {
+            auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
+            result->final_position = current_position_;
+            result->reached_goal = false;
+            result->stalled = true;
+            current_goal_handle_->abort(result);
+            current_goal_handle_.reset();  // Clear the current goal handle
+            process_next_goal();  // Process the next goal, if any
+        }
+
+        // Check if the gripper has stopped after a cancel request
+        if (cancel_requested_ && is_stopped()) {
+        auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
+        result->final_position = current_position_;
+        result->reached_goal = false;
+        result->stalled = false;
+        current_goal_handle_->canceled(result);  // Notify that the goal was canceled successfully
+        cancel_requested_ = false;  // Reset the cancel requested flag
+        current_goal_handle_.reset();  // Clear the current goal handle
+        process_next_goal();  // Proceed to the next goal if available
+
+        RCLCPP_INFO(get_node()->get_logger(), "Gripper Action was canceled and motion has stopped!");
+        }
+
+        // Publish feedback about the current position
+        publish_feedback();
+
+        // Check if the target position is reached within a tolerance
+        if (std::abs(current_position_ - target_position_) < POSITION_TOLERANCE) {
+            auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
+            result->final_position = current_position_;
+            result->reached_goal = true;
+            result->stalled = false;
+            current_goal_handle_->succeed(result);
+            current_goal_handle_.reset();  // Clear the current goal handle
+            process_next_goal();  // Process the next goal, if any
+            target_position_ = -1.0;
+        }
+    }
+
+    return controller_interface::return_type::OK;
+}
+
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_deactivate(const rclcpp_lifecycle::State & )
+{
+    RCLCPP_INFO(get_node()->get_logger(), "Deactivating and releasing interfaces.");
+    release_interfaces();  // Ensure this function properly cleans up resources
+    joint_position_command_interface_.clear();
+    max_velocity_command_interface_.clear();
+    max_effort_command_interface_.clear();
+    joint_position_state_interface_.clear();
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_cleanup(const rclcpp_lifecycle::State & )
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_error(const rclcpp_lifecycle::State & )
+{
+    RCLCPP_ERROR(get_node()->get_logger(), "An error occurred, attempting to reset the controller");
+
+    // Attempt to stop any ongoing gripper motion as a safety precaution
+    try {
+        stop_motion(joint_position_state_interface_[0].get().get_value());  // Assume stop_motion sets the gripper commands to a safe state
+    } catch (const std::exception& e) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Failed to stop the gripper during error handling: %s", e.what());
+        return controller_interface::CallbackReturn::ERROR;  // Fail if stopping motion fails
+    }
+
+    // Log that the controller is attempting to reactivate
+    RCLCPP_INFO(get_node()->get_logger(), "Attempting to reactivate the controller");
+
+    // Directly call the on_activate method to reset and reactivate the controller
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_shutdown(const rclcpp_lifecycle::State & )
+{
+  return CallbackReturn::SUCCESS;
+}
+
+// Action Handling
+rclcpp_action::GoalResponse RobotiqGripperCommandController::handle_goal(const rclcpp_action::GoalUUID & goal_uuid,
+     std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal)
+{
+    (void)goal_uuid;  // Suppress unused variable warning
+    RCLCPP_INFO(get_node()->get_logger(), "Received goal request with position: %f", goal->desired_position);
+
+    if (!is_valid_goal(goal)) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Goal is invalid");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+}
+
+rclcpp_action::CancelResponse RobotiqGripperCommandController::handle_cancel(const std::shared_ptr<GripperGoalHandle> goal_handle)
+{
+    if (current_goal_handle_ == goal_handle) {
+        RCLCPP_INFO(get_node()->get_logger(), "Received request to cancel goal");
+        stop_motion(joint_position_state_interface_[0].get().get_value());  // Attempt to stop the gripper immediately
+        cancel_requested_ = true;  // Set a flag indicating cancellation has been requested
+        return rclcpp_action::CancelResponse::ACCEPT;  // Accept the cancellation, pending completion
+    }
+    return rclcpp_action::CancelResponse::REJECT;  // Reject cancellation if it's not the current goal
+}
+
+void RobotiqGripperCommandController::handle_accepted(const std::shared_ptr<GripperGoalHandle> goal_handle)
+{
+
+    goal_queue_.push(goal_handle);
+    if (!current_goal_handle_) {  // Only start a new goal if one isn't already active
+        process_next_goal();
+    }
+}
+
+void RobotiqGripperCommandController::publish_feedback()
+{
+    if (!current_goal_handle_) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Goal handle is null, cannot publish feedback.");
+        return;
+    }
+
+    if (current_goal_handle_->is_canceling()) {
+        RCLCPP_WARN(get_node()->get_logger(), "Goal is being canceled, not publishing feedback.");
+        return;
+    }
+
+    auto feedback = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Feedback>();
+    feedback->current_position = current_position_;
+
+    try {
+        current_goal_handle_->publish_feedback(feedback);
+    } catch (const std::exception& e) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Failed to publish feedback: %s", e.what());
+    }
+}
+
+void RobotiqGripperCommandController::process_next_goal()
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (goal_queue_.empty()) {
+        current_goal_handle_.reset();
+        RCLCPP_DEBUG(get_node()->get_logger(), "No more goals to process.");
+        return;
+    }
+    current_goal_handle_ = goal_queue_.front();
+    goal_queue_.pop();
+    current_goal_handle_->execute();
+
+    if (current_goal_handle_) {
+        auto goal = current_goal_handle_->get_goal();
+        if (!goal) {
+            RCLCPP_ERROR(get_node()->get_logger(), "Received a null goal in queue, skipping...");
+            process_next_goal();  // Recursively handle the next goal
+        }
+        else {
+            try {
+                auto goal = current_goal_handle_->get_goal();
+                target_position_ = goal->desired_position;
+                double desired_max_velocity = goal->max_velocity;
+                double desired_max_effort = goal->max_effort;
+
+                if (!command_interface_checker_())
+                {
+                    throw std::runtime_error("At least one interface is not initialized!");
+                }
+
+                joint_position_command_interface_[0].get().set_value(target_position_);
+                max_velocity_command_interface_[0].get().set_value(desired_max_velocity);
+                max_effort_command_interface_[0].get().set_value(desired_max_effort);
+            }
+            catch (const std::exception& e) {
+                RCLCPP_ERROR(get_node()->get_logger(), "Exception in process_next_goal: %s", e.what());
+            }
+        }
+    }
+}
+
+// Utility
+bool RobotiqGripperCommandController::command_interface_checker_()
+{
+    if (joint_position_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No position command interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Position command interfaces have been successfully initialized.");
+    }
+
+    if (max_velocity_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No velocity command interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Velocity command interfaces have been successfully initialized.");
+    }
+
+    if (max_effort_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No effort command interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Effort command interfaces have been successfully initialized.");
+    }
+
+    if (joint_position_state_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No position state interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Position stateinterfaces have been successfully initialized.");
+    }
+
+    return true;
+}
+
+bool RobotiqGripperCommandController::is_valid_goal(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal) {
+    // Add your validation logic here
+    return goal->desired_position >= 0 && goal->desired_position <= max_position_;
+}
+
+bool RobotiqGripperCommandController::is_stalled(double current_position)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (std::abs(current_position - last_position_) < POSITION_TOLERANCE) {
+        stall_counter_++;
+    } else {
+        stall_counter_ = 0;  // Reset the stall counter if there is movement
+    }
+    last_position_ = current_position;
+
+    return stall_counter_ > MAX_STALL_COUNT;
+}
+
+bool RobotiqGripperCommandController::stop_motion(double current_position)
+{
+    joint_position_command_interface_[0].get().set_value(current_position);
+    max_velocity_command_interface_[0].get().set_value(MIN_SPEED);
+    max_effort_command_interface_[0].get().set_value(MIN_EFFORT);
+
+    return true;
+}
+
+bool RobotiqGripperCommandController::is_stopped()
+{
+    double current_position = joint_position_state_interface_[0].get().get_value();
+    bool stopped = std::abs(current_position - last_position_) < POSITION_TOLERANCE;
+    last_position_ = current_position;
+    return stopped;
+}
+
+
+}
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqGripperCommandController, controller_interface::ControllerInterface)
+

src/robotiq_2f/robotiq_2f_description/CMakeLists.txt

@@ -0,0 +1,20 @@
+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_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY
+    config
+    launch
+    meshes
+    rviz
+    urdf
+  DESTINATION
+    share/${PROJECT_NAME}
+)
+ament_package()

src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml

@@ -0,0 +1,9 @@
+robotiq_2f_gripper:
+  # Physical limits
+  min_position: 0.0     # Meters (fully closed)
+  max_position: 0.1426    # Meters (fully open)
+  max_angle: 0.695      # radiants (closed)
+  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,8 @@
+robotiq_2f_gripper:
+  # 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/config/robotiq_controllers.yaml

@@ -0,0 +1,24 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500  # Hz
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    forward_gripper_position_controller:
+      type: robotiq_2f_controllers/ForwardController
+    robotiq_gripper_controller:
+      type: robotiq_2f_controllers/GripperCommand
+    robotiq_activation_controller:
+      type: robotiq_2f_controllers/RobotiqActivationController
+
+forward_gripper_position_controller:
+  ros__parameters:
+    joint: $(var prefix)gripper_gap
+
+robotiq_gripper_controller:
+  ros__parameters:
+    joint: $(var prefix)gripper_gap
+    max_gap: 0.14
+
+robotiq_activation_controller:
+  ros__parameters:
+    default: true

src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py

@@ -0,0 +1,186 @@
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.conditions import IfCondition
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+import os
+
+def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="robotiq_2f_description",
+            description="Description package with gripper URDF/XACRO files.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_file",
+            default_value="robotiq_2f_140.urdf.xacro",
+            description="URDF/XACRO description file with the robot gripper.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value="",
+            description="prefix of the joint names, useful for \
+        multi-robot setup. If changed, also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start gripper with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip", description="IP address by which the robot can be reached."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_port",
+            default_value="63352",
+            description="Remote port for UR Cap.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_rviz", default_value="false", description="Launch RViz?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("use_forward_controller",
+                              default_value="false",
+                              description="Use forward controller?")
+    )
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
+
+
+def launch_setup(context, *args, **kwargs):
+
+    robot_ip = LaunchConfiguration("robot_ip")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    prefix = LaunchConfiguration("prefix")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    robot_port = LaunchConfiguration("robot_port")
+    launch_rviz = LaunchConfiguration("launch_rviz")
+    use_forward_controller = LaunchConfiguration("use_forward_controller")
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
+            " ",
+            "prefix:=",
+            prefix,
+            " ",
+            "use_fake_hardware:=",
+            use_fake_hardware,
+            " ",
+            "robot_ip:=",
+            robot_ip,
+            " ",
+            "robot_port:=",
+            robot_port,
+            " ",
+        ]
+    )
+
+    robot_description = {"robot_description": robot_description_content}
+
+    controllers_file = "robotiq_controllers.yaml"
+    initial_joint_controllers = PathJoinSubstitution(
+        [FindPackageShare(description_package), "config", controllers_file]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[
+            robot_description,
+            ParameterFile(initial_joint_controllers, allow_substs=True),
+        ],
+        output='screen',
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[robot_description],
+        output='screen',
+    )
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(description_package), "rviz", "view_robot.rviz"]
+    )
+
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        condition=IfCondition(launch_rviz),
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+        output='screen',
+    )
+
+    robotiq_activation_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["robotiq_activation_controller", "-c", "/controller_manager"],
+        output='screen',
+    )
+
+    def controller_spawner(name, active=True):
+        inactive_flags = ["--inactive"] if not active else []
+        return Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=[
+                          name,
+                          "--controller-manager",
+                          "/controller_manager",
+                      ] + inactive_flags,
+            output='screen',
+        )
+
+    if use_forward_controller.perform(context) == 'false':
+        controller_spawner_names = ['robotiq_gripper_controller']
+        controller_spawner_inactive_names = ["forward_gripper_position_controller"]
+    else:
+        controller_spawner_names = ["forward_gripper_position_controller"]
+        controller_spawner_inactive_names = ['robotiq_gripper_controller']
+
+    controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
+        controller_spawner(name, active=False) for name in controller_spawner_inactive_names
+    ]
+
+    nodes = [
+        control_node,
+        robot_state_publisher_node,
+        joint_state_broadcaster_spawner,
+        robotiq_activation_controller_spawner,
+        rviz_node,
+    ] + controller_spawners
+
+    return nodes

src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py

@@ -0,0 +1,75 @@
+import launch
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    LaunchConfiguration,
+    PathJoinSubstitution,
+)
+import launch_ros
+import os
+
+
+def generate_launch_description():
+    pkg_share = launch_ros.substitutions.FindPackageShare(
+        package="robotiq_2f_description"
+    ).find("robotiq_2f_description")
+    default_model_path = os.path.join(
+        pkg_share, "urdf", "robotiq_2f_140.urdf.xacro"
+    )
+    default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_robot.rviz")
+
+    args = []
+    args.append(
+        launch.actions.DeclareLaunchArgument(
+            name="model",
+            default_value=default_model_path,
+            description="Absolute path to gripper URDF file",
+        )
+    )
+    args.append(
+        launch.actions.DeclareLaunchArgument(
+            name="rvizconfig",
+            default_value=default_rviz_config_path,
+            description="Absolute path to rviz config file",
+        )
+    )
+
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            LaunchConfiguration("model"),
+        ]
+    )
+    robot_description_param = {
+        "robot_description": launch_ros.parameter_descriptions.ParameterValue(
+            robot_description_content, value_type=str
+        )
+    }
+
+    robot_state_publisher_node = launch_ros.actions.Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[robot_description_param],
+    )
+
+    joint_state_publisher_node = launch_ros.actions.Node(
+        package="joint_state_publisher_gui",
+        executable="joint_state_publisher_gui",
+    )
+
+    rviz_node = launch_ros.actions.Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="screen",
+        arguments=["-d", LaunchConfiguration("rvizconfig")],
+    )
+
+    nodes = [
+        robot_state_publisher_node,
+        joint_state_publisher_node,
+        rviz_node,
+    ]
+
+    return launch.LaunchDescription(args + nodes)

src/robotiq_2f/robotiq_2f_description/package.xml

@@ -0,0 +1,35 @@
+<?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>
+
+  <exec_depend>joint_state_publisher_gui</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>urdf</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <exec_depend>robotiq_2f_msgs</exec_depend>
+
+  <exec_depend>ros2_control</exec_depend>
+  <exec_depend>ros2_controllers</exec_depend>
+
+  <exec_depend>robotiq_2f_controllers</exec_depend>
+  <exec_depend>gripper_controllers</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_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/rviz/view_robot.rviz

@@ -0,0 +1,228 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /RobotModel1
+      Splitter Ratio: 0.5
+    Tree Height: 555
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz_default_plugins/RobotModel
+      Collision Enabled: false
+      Description File: ""
+      Description Source: Topic
+      Description Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /robot_description
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        dummy_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_left_inner_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_left_inner_finger_pad:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_left_inner_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_left_outer_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_left_outer_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_right_inner_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_right_inner_finger_pad:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_right_inner_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_right_outer_finger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_140_right_outer_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
+      Name: RobotModel
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 1.01347017288208
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.3253982663154602
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 1.3853975534439087
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1200
+  X: 70
+  Y: 60

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro

@@ -0,0 +1,123 @@
+<?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
+        fake_sensor_commands
+        robot_ip
+        robot_port
+        min_position
+        max_position
+        max_angle
+        min_speed
+        max_speed
+        min_force
+        max_force">
+
+        <ros2_control name="${name}" type="system">
+            <!-- Plugins -->
+            <hardware>
+                 <xacro:if value="${use_fake_hardware}">
+                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
+                    <param name="use_fake_hardware">${use_fake_hardware}</param>
+                    <param name="mock_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.695</param>
+                    <param name="robot_ip">${robot_ip}</param>
+                    <param name="robot_port">${robot_port}</param>
+                    <param name="use_fake_hardware">${use_fake_hardware}</param>
+                </xacro:unless>
+                <param name="gripper_speed_multiplier">1.0</param>
+                <param name="gripper_force_multiplier">0.5</param>
+                <param name="min_position">${min_position}</param>
+                <param name="max_position">${max_position}</param>
+                <param name="max_angle">${max_angle}</param>
+                <param name="min_speed">${min_speed}</param>
+                <param name="max_speed">${max_speed}</param>
+                <param name="min_force">${min_force}</param>
+                <param name="max_force">${max_force}</param>
+            </hardware>
+
+            <!-- Joint interfaces -->
+            <!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
+            <joint name="${prefix}gripper_gap">
+                <command_interface name="position" />
+                <state_interface name="position">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${prefix}finger_joint">
+                <state_interface name="position">
+                    <param name="initial_value">0.695</param>
+                </state_interface>
+            </joint>
+            <!-- When simulating we need to include the rest of the gripper joints -->
+            <!-- <xacro:if value="${use_fake_hardware}"> -->
+            <xacro:if value="false">
+                <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}left_outer_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_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}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}right_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_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>
+            </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_description/urdf/robotiq_2f_140.urdf.xacro

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
+    <!-- parameters -->
+    <xacro:arg name="use_fake_hardware" default="false" />
+
+    <!-- Import macros -->
+    <xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
+
+    <link name="world" />
+    <xacro:robotiq_gripper name="RobotiqGripperHardwareInterface" prefix="" parent="world" use_fake_hardware="$(arg use_fake_hardware)">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:robotiq_gripper>
+</robot>

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro

@@ -0,0 +1,468 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
+    <xacro:macro name="robotiq_gripper" params="
+        name
+        prefix
+        parent
+        *origin
+        include_ros2_control:=true
+        use_fake_hardware:=false
+        fake_sensor_commands:=false
+        robot_ip:=192.168.1.223
+        robot_port:=63352">
+
+
+        <xacro:property name="config_file" value="$(find robotiq_2f_description)/config/robotiq_2f_140_config.yaml" />
+        <xacro:property name="config_dict" value="${xacro.load_yaml(config_file)}" />
+        <xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
+        <xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
+        <xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
+        <xacro:property name="max_angle" value="${robotiq_2f_gripper_dict['max_angle']}" />
+        <xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
+        <xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
+        <xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
+        <xacro:property name="max_force" value="${robotiq_2f_gripper_dict['max_force']}" />
+        <!-- ros2 control include -->
+        <xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_140.ros2_control.xacro" />
+        <!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
+        <xacro:if value="${include_ros2_control}">
+            <xacro:robotiq_gripper_ros2_control
+                name="${name}" prefix="${prefix}"
+                use_fake_hardware="${use_fake_hardware}"
+                fake_sensor_commands="${fake_sensor_commands}"
+                robot_ip="${robot_ip}"
+                robot_port="${robot_port}"
+                min_position="${min_position}"
+                max_position="${max_position}"
+                max_angle="${max_angle}"
+                min_speed="${min_speed}"
+                max_speed="${max_speed}"
+                min_force="${min_force}"
+                max_force="${max_force}"/>
+        </xacro:if>
+
+        <!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->
+        <link name="${prefix}robotiq_base_link"/>
+        <joint name="${prefix}robotiq_base_joint" type="fixed">
+          <parent link="${parent}" />
+          <child link="${prefix}robotiq_base_link" />
+          <origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
+        </joint>
+
+        <link name="${prefix}robotiq_140_base_link">
+            <inertial>
+                <origin xyz="8.625E-08 -4.6583E-06 0.03145" rpy="0 0 0" />
+                <mass value="0.22652" />
+                <inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_base_link.stl" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.1 0.1 0.1 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_base_link.stl" />
+                </geometry>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_left_outer_knuckle">
+            <inertial>
+                <origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
+                <mass value="0.00853198276973456" />
+                <inertia
+                    ixx="2.89328108496468E-06"
+                    ixy="-1.57935047237397E-19"
+                    ixz="-1.93980378593255E-19"
+                    iyy="1.86719750325683E-06"
+                    iyz="-1.21858577871576E-06"
+                    izz="1.21905238907251E-06" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
+                </geometry>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_right_outer_knuckle">
+            <inertial>
+                <origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
+                <mass value="0.00853198276973456" />
+                <inertia
+                    ixx="2.89328108496468E-06"
+                    ixy="-1.57935047237397E-19"
+                    ixz="-1.93980378593255E-19"
+                    iyy="1.86719750325683E-06"
+                    iyz="-1.21858577871576E-06"
+                    izz="1.21905238907251E-06" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+                </material>
+            </visual>
+            <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
+                </geometry>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_left_outer_finger">
+            <inertial>
+                <origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
+                <mass value="0.022614240507152" />
+                <inertia
+                    ixx="1.52518312458174E-05"
+                    ixy="9.76583423954399E-10"
+                    ixz="-5.43838577022588E-10"
+                    iyy="6.17694243867776E-06"
+                    iyz="6.78636130740228E-06"
+                    izz="1.16494917907219E-05" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.1 0.1 0.1 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
+                </geometry>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_right_outer_finger">
+            <inertial>
+                <origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
+                <mass value="0.022614240507152" />
+                <inertia
+                    ixx="1.52518312458174E-05"
+                    ixy="9.76583423954399E-10"
+                    ixz="-5.43838577022588E-10"
+                    iyy="6.17694243867776E-06"
+                    iyz="6.78636130740228E-06"
+                    izz="1.16494917907219E-05" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.1 0.1 0.1 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
+                </geometry>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_left_inner_knuckle">
+            <inertial>
+                <origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
+                <mass value="0.0271177346495152" />
+                <inertia
+                    ixx="2.61910379223783E-05"
+                    ixy="-2.43616858946494E-07"
+                    ixz="-6.37789906117123E-09"
+                    iyy="2.8270243746167E-06"
+                    iyz="-5.37200748039765E-07"
+                    izz="2.83695868220296E-05" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.1 0.1 0.1 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
+                </geometry>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_right_inner_knuckle">
+            <inertial>
+                <origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
+                <mass value="0.0271177346495152" />
+                <inertia
+                    ixx="2.61910379223783E-05"
+                    ixy="-2.43616858946494E-07"
+                    ixz="-6.37789906117123E-09"
+                    iyy="2.8270243746167E-06"
+                    iyz="-5.37200748039765E-07"
+                    izz="2.83695868220296E-05" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.1 0.1 0.1 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
+                </geometry>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_left_inner_finger">
+            <inertial>
+                <origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
+                <mass value="0.0104003125914103" />
+                <inertia
+                    ixx="2.71909453810972E-06"
+                    ixy="1.35402465472579E-21"
+                    ixz="-7.1817349065269E-22"
+                    iyy="7.69100314106116E-07"
+                    iyz="6.74715432769696E-07"
+                    izz="2.30315190420171E-06" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.1 0.1 0.1 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
+                </geometry>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_right_inner_finger">
+            <inertial>
+                <origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
+                <mass value="0.0104003125914103" />
+                <inertia
+                    ixx="2.71909453810972E-06"
+                    ixy="1.35402465472579E-21"
+                    ixz="-7.1817349065269E-22"
+                    iyy="7.69100314106116E-07"
+                    iyz="6.74715432769696E-07"
+                    izz="2.30315190420171E-06" />
+            </inertial>
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
+                </geometry>
+                <material name="">
+                    <color rgba="0.1 0.1 0.1 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
+                </geometry>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_left_inner_finger_pad">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <box size="0.027 0.065 0.0075"/>
+                </geometry>
+                <material name="">
+                    <color rgba="0.9 0.9 0.9 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <box size="0.03 0.07 0.0075"/>
+                </geometry>
+                <material name="">
+                    <color rgba="0.9 0.0 0.0 1" />
+                </material>
+            </collision>
+        </link>
+
+        <link name="${prefix}robotiq_140_right_inner_finger_pad">
+            <visual>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <box size="0.027 0.065 0.0075"/>
+                </geometry>
+                <material name="">
+                    <color rgba="0.9 0.9 0.9 1" />
+                </material>
+            </visual>
+            <collision>
+                <origin xyz="0 0 0" rpy="0 0 0" />
+                <geometry>
+                    <box size="0.03 0.07 0.0075"/>
+                </geometry>
+                <material name="">
+                    <color rgba="0.9 0.0 0.0 1" />
+                </material>
+            </collision>
+        </link>
+
+
+
+        <!-- Joints-->
+        <joint name="${prefix}robotiq_140_base_joint" type="fixed">
+            <parent link="${prefix}robotiq_base_link" />
+            <child link="${prefix}robotiq_140_base_link" />
+            <xacro:insert_block name="origin" />
+        </joint>
+
+        <joint name="${prefix}finger_joint" type="revolute">
+            <origin xyz="0 -0.030601 0.054905" rpy="${pi / 2 + .725} 0 0" />
+            <parent link="${prefix}robotiq_140_base_link" />
+            <child link="${prefix}robotiq_140_left_outer_knuckle" />
+            <axis xyz="-1 0 0" />
+            <limit lower="0" upper="0.7" velocity="2.0" effort="1000" />
+        </joint>
+
+        <joint name="${prefix}right_outer_knuckle_joint" type="revolute">
+            <origin xyz="0 0.030601 0.054905" rpy="${pi / 2 + .725} 0 ${pi}" />
+            <parent link="${prefix}robotiq_140_base_link" />
+            <child link="${prefix}robotiq_140_right_outer_knuckle" />
+            <axis xyz="1 0 0" />
+            <limit lower="-0.725" upper="0.725" velocity="2.0" effort="1000" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
+        </joint>
+
+        <joint name="${prefix}left_outer_finger_joint" type="fixed">
+            <origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
+            <parent link="${prefix}robotiq_140_left_outer_knuckle" />
+            <child link="${prefix}robotiq_140_left_outer_finger" />
+            <axis xyz="1 0 0" />
+        </joint>
+
+        <joint name="${prefix}right_outer_finger_joint" type="fixed">
+            <origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
+            <parent link="${prefix}robotiq_140_right_outer_knuckle" />
+            <child link="${prefix}robotiq_140_right_outer_finger" />
+            <axis xyz="1 0 0" />
+        </joint>
+
+        <joint name="${prefix}left_inner_knuckle_joint" type="revolute">
+            <origin xyz="0 -0.0127 0.06142" rpy="${pi / 2 + .725} 0 0" />
+            <parent link="${prefix}robotiq_140_base_link" />
+            <child link="${prefix}robotiq_140_left_inner_knuckle" />
+            <axis xyz="1 0 0" />
+            <limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
+        </joint>
+
+        <joint name="${prefix}right_inner_knuckle_joint" type="revolute">
+            <origin xyz="0 0.0127 0.06142" rpy="${pi / 2 + .725} 0 ${-pi}" />
+            <parent link="${prefix}robotiq_140_base_link" />
+            <child link="${prefix}robotiq_140_right_inner_knuckle" />
+            <axis xyz="1 0 0" />
+            <limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
+        </joint>
+
+        <joint name="${prefix}left_inner_finger_joint" type="revolute">
+            <origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
+            <parent link="${prefix}robotiq_140_left_outer_finger" />
+            <child link="${prefix}robotiq_140_left_inner_finger" />
+            <axis xyz="1 0 0" />
+            <limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
+            <mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
+        </joint>
+
+        <joint name="${prefix}right_inner_finger_joint" type="revolute">
+            <origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
+            <parent link="${prefix}robotiq_140_right_outer_finger" />
+            <child link="${prefix}robotiq_140_right_inner_finger" />
+            <axis xyz="1 0 0" />
+            <limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
+            <mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
+        </joint>
+
+        <joint name="${prefix}left_inner_finger_pad_joint" type="fixed">
+            <origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
+            <parent link="${prefix}robotiq_140_left_inner_finger" />
+            <child link="${prefix}robotiq_140_left_inner_finger_pad" />
+            <axis xyz="0 0 1" />
+        </joint>
+
+        <joint name="${prefix}right_inner_finger_pad_joint" type="fixed">
+            <origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
+            <parent link="${prefix}robotiq_140_right_inner_finger" />
+            <child link="${prefix}robotiq_140_right_inner_finger_pad" />
+            <axis xyz="0 0 1" />
+        </joint>
+
+        <!-- Dummy link to facilitate the pseudo joint -->
+        <link name="dummy_link">
+            <visual>
+                <geometry>
+                    <box size="0.001 0.001 0.001"/>  <!-- Very small, effectively invisible -->
+                </geometry>
+                <material name="transparent">
+                    <color rgba="0.0 0.0 0.0 0.0"/>  <!-- Transparent color -->
+                </material>
+            </visual>
+        </link>
+
+        <!-- Fixed joint to attach the dummy link to part of the gripper -->
+        <joint name="dummy_to_pad" type="fixed">
+            <parent link="dummy_link"/>
+            <child link="robotiq_140_left_inner_finger_pad"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Position it appropriately -->
+        </joint>
+
+        <!-- Pseudo prismatic joint for controlling the gripper gap using the dummy link -->
+        <joint name="gripper_gap" type="prismatic">
+            <parent link="robotiq_140_base_link"/>  <!-- Adjust the parent link as necessary -->
+            <child link="dummy_link"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Properly position this joint -->
+            <axis xyz="0 1 1"/>  <!-- Ensure the axis aligns with the intended motion -->
+            <limit lower="0.0" upper="0.14" effort="235" velocity="0.15"/>
+        </joint>
+
+    </xacro:macro>
+</robot>

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro

@@ -0,0 +1,108 @@
+<?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
+        fake_sensor_commands
+        robot_ip
+        robot_port
+        min_position
+        max_position
+        max_angle
+        min_speed
+        max_speed
+        min_force
+        max_force">
+
+        <ros2_control name="${name}" type="system">
+            <!-- Plugins -->
+            <hardware>
+                 <xacro:if value="${use_fake_hardware}">
+                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
+                    <param name="use_fake_hardware">${use_fake_hardware}</param>
+                    <param name="mock_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.7929</param>
+                    <param name="robot_ip">${robot_ip}</param>
+                    <param name="robot_port">${robot_port}</param>
+                    <param name="use_fake_hardware">${use_fake_hardware}</param>
+                </xacro:unless>
+                <param name="gripper_speed_multiplier">1.0</param>
+                <param name="gripper_force_multiplier">0.5</param>
+                <param name="min_position">${min_position}</param>
+                <param name="max_position">${max_position}</param>
+                <param name="max_angle">${max_angle}</param>
+                <param name="min_speed">${min_speed}</param>
+                <param name="max_speed">${max_speed}</param>
+                <param name="min_force">${min_force}</param>
+                <param name="max_force">${max_force}</param>
+            </hardware>
+
+            <!-- Joint interfaces -->
+            <!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
+            <joint name="${prefix}gripper_gap">
+                <command_interface name="position" />
+                <state_interface name="position">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${prefix}finger_joint">
+                <state_interface name="position">
+                    <param name="initial_value">0.7929</param>
+                </state_interface>
+            </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_description/urdf/robotiq_2f_85.urdf.xacro

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
+    <!-- parameters -->
+    <xacro:arg name="use_fake_hardware" default="false" />
+
+    <!-- Import macros -->
+    <xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
+
+    <link name="world" />
+    <xacro:robotiq_gripper name="RobotiqGripperHardwareInterface" prefix="" parent="world" use_fake_hardware="$(arg use_fake_hardware)">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+    </xacro:robotiq_gripper>
+</robot>

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro

@@ -0,0 +1,308 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
+    <xacro:macro name="robotiq_gripper" params="
+        name
+        prefix
+        parent
+        *origin
+        include_ros2_control:=true
+        use_fake_hardware:=false
+        fake_sensor_commands:=false
+        robot_ip:=192.168.1.1
+        robot_port:=63352">
+
+        <xacro:property name="config_file" value="$(find robotiq_2f_description)/config/robotiq_2f_85_config.yaml" />
+        <xacro:property name="config_dict" value="${xacro.load_yaml(config_file)}" />
+        <xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
+        <xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
+        <xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
+        <xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
+        <xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
+        <xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
+        <xacro:property name="max_force" value="${robotiq_2f_gripper_dict['max_force']}" />
+
+        <!-- ros2 control include -->
+        <xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_85.ros2_control.xacro" />
+        <!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
+        <xacro:if value="${include_ros2_control}">
+            <xacro:robotiq_gripper_ros2_control
+                name="${name}" prefix="${prefix}"
+                use_fake_hardware="${use_fake_hardware}"
+                fake_sensor_commands="${fake_sensor_commands}"
+                robot_ip="${robot_ip}"
+                robot_port="${robot_port}"
+                min_position="${min_position}"
+                max_position="${max_position}"
+                min_speed="${min_speed}"
+                max_speed="${max_speed}"
+                min_force="${min_force}"
+                max_force="${max_force}"/>
+        </xacro:if>
+
+        <link name="${prefix}robotiq_85_base_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/robotiq_base.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/robotiq_base.stl" />
+                </geometry>
+            </collision>
+            <inertial>
+                <origin xyz="0.0 2.274e-05 0.03232288" rpy="0 0 0" />
+                <mass value="6.6320197e-01" />
+                <inertia ixx="5.1617816e-04" iyy="5.8802208e-04" izz="3.9462776e-04" ixy="2.936e-8" ixz="0.0" iyz="-3.2296e-7" />
+            </inertial>
+        </link>
+
+        <link name="${prefix}robotiq_85_left_knuckle_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_knuckle.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_knuckle.stl" />
+                </geometry>
+            </collision>
+            <inertial>
+                <origin xyz="0.01213197 0.0002 -0.00058647" rpy="0 0 0" />
+                <mass value="1.384773208e-02" />
+                <inertia ixx="3.5232e-7" iyy="2.31944e-6" izz="2.23136e-6" ixy="0.0" ixz="1.1744e-7" iyz="0" />
+            </inertial>
+        </link>
+
+        <link name="${prefix}robotiq_85_right_knuckle_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_knuckle.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_knuckle.stl" />
+                </geometry>
+            </collision>
+            <inertial>
+                <origin xyz="-0.01213197 0.0002 -0.00058647" rpy="0 0 0" />
+                <mass value="1.384773208e-02" />
+                <inertia ixx="3.5232e-7" iyy="2.31944e-6" izz="2.23136e-6" ixy="0.0" ixz="-1.1744e-7" iyz="0.0" />
+            </inertial>
+        </link>
+
+        <link name="${prefix}robotiq_85_left_finger_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_finger.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_finger.stl" />
+                </geometry>
+            </collision>
+            <inertial>
+                <origin xyz="0.00346899 -0.00079447 0.01867121" rpy="0 0 0" />
+                <mass value="4.260376752e-02" />
+                <inertia ixx="1.385792000000000e-05" iyy="1.183208e-05" izz="5.19672e-06" ixy="0.0" ixz="-2.17264e-06" iyz="0.0" />
+            </inertial>
+        </link>
+
+        <link name="${prefix}robotiq_85_right_finger_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_finger.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_finger.stl" />
+                </geometry>
+            </collision>
+            <inertial>
+                <origin xyz="-0.00346899 -5.53e-06 0.01867121" rpy="0 0 0" />
+                <mass value="4.260376752000000e-02" />
+                <inertia ixx="1.385792e-05" iyy="1.183208e-05" izz="5.19672e-06" ixy="0.0" ixz="2.17264e-06" iyz="0.0" />
+            </inertial>
+        </link>
+
+        <link name="${prefix}robotiq_85_left_inner_knuckle_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_inner_knuckle.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_inner_knuckle.stl" />
+                </geometry>
+            </collision>
+            <inertial>
+                <origin xyz="0.01897699 0.00015001 0.02247101" rpy="0 0 0" />
+                <mass value="2.969376448e-02" />
+                <inertia ixx="9.57136e-06" iyy="8.69056e-06" izz="8.19144e-06" ixy="0.0" ixz="-3.93424e-06" iyz="0.0" />
+            </inertial>
+        </link>
+
+        <link name="${prefix}robotiq_85_right_inner_knuckle_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_inner_knuckle.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_inner_knuckle.stl" />
+                </geometry>
+            </collision>
+            <inertial>
+                <origin xyz="-0.01926824 5.001e-05 0.02222178" rpy="0 0 0" />
+                <mass value="2.969376448e-02" />
+                <inertia ixx="9.42456e-06" iyy="8.69056e-06" izz="8.33824e-06" ixy="0.0" ixz="3.9636e-06" iyz="0.0" />
+            </inertial>
+        </link>
+
+        <link name="${prefix}robotiq_85_left_finger_tip_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_finger_tip.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_finger_tip.stl" />
+                </geometry>
+                <surface>
+                    <friction>
+                    <ode>
+                        <mu1>100000.0</mu1>
+                        <mu2>100000.0</mu2>
+                    </ode>
+                    </friction>
+                    <contact>
+                    <ode>
+                        <kp>1e+5</kp>
+                        <kd>1</kd>
+                        <soft_cfm>0</soft_cfm>
+                        <soft_erp>0.2</soft_erp>
+                        <minDepth>0.002</minDepth>
+                        <maxVel>0</maxVel>
+                    </ode>
+                    </contact>
+                </surface>
+            </collision>
+            <inertial>
+                <origin xyz="-0.01456706 -0.0008 0.01649701" rpy="0 0 0" />
+                <mass value="4.268588744e-02" />
+                <inertia ixx="1.048152e-05" iyy="1.197888e-05" izz="4.22784e-06" ixy="0.0" ixz="3.5232e-6" iyz="0.0" />
+            </inertial>
+        </link>
+
+        <link name="${prefix}robotiq_85_right_finger_tip_link">
+            <visual>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_finger_tip.dae" />
+                </geometry>
+            </visual>
+            <collision>
+                <geometry>
+                    <mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_finger_tip.stl" />
+                </geometry>
+                <surface>
+                    <friction>
+                    <ode>
+                        <mu1>100000.0</mu1>
+                        <mu2>100000.0</mu2>
+                    </ode>
+                    </friction>
+                    <contact>
+                    <ode>
+                        <kp>1e+5</kp>
+                        <kd>1</kd>
+                        <soft_cfm>0</soft_cfm>
+                        <soft_erp>0.2</soft_erp>
+                        <minDepth>0.002</minDepth>
+                        <maxVel>0</maxVel>
+                    </ode>
+                    </contact>
+                </surface>
+            </collision>
+            <inertial>
+                <origin xyz="0.01456706 5e-05 0.01649701" rpy="0 0 0" />
+                <mass value="4.268588744e-02" />
+                <inertia ixx="1.048152e-05" iyy="1.197888e-05" izz="4.22784e-06" ixy="0.0" ixz="-3.5232e-06" iyz="0.0" />
+            </inertial>
+        </link>
+
+        <!-- Joints-->
+        <joint name="${prefix}robotiq_85_base_joint" type="fixed">
+            <parent link="${parent}" />
+            <child link="${prefix}robotiq_85_base_link" />
+            <xacro:insert_block name="origin" />
+        </joint>
+
+        <joint name="${prefix}finger_joint" type="revolute">
+            <parent link="${prefix}robotiq_85_base_link" />
+            <child link="${prefix}robotiq_85_left_knuckle_link" />
+            <axis xyz="0 -1 0" />
+            <origin xyz="0.03060114 0.0 0.05490452" rpy="0 0 0" />
+            <limit lower="0.0" upper="0.8" velocity="0.5" effort="50" />
+        </joint>
+
+        <joint name="${prefix}right_outer_knuckle_joint" type="revolute">
+            <parent link="${prefix}robotiq_85_base_link" />
+            <child link="${prefix}robotiq_85_right_knuckle_link" />
+            <axis xyz="0 -1 0" />
+            <origin xyz="-0.03060114 0.0 0.05490452" rpy="0 0 0" />
+            <limit lower="-0.8" upper="0.0" velocity="0.5" effort="50" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" />
+        </joint>
+
+        <joint name="${prefix}left_outer_finger_joint" type="fixed">
+            <parent link="${prefix}robotiq_85_left_knuckle_link" />
+            <child link="${prefix}robotiq_85_left_finger_link" />
+            <origin xyz="0.03152616 0.0 -0.00376347" rpy="0 0 0" />
+        </joint>
+
+        <joint name="${prefix}right_outer_finger_joint" type="fixed">
+            <parent link="${prefix}robotiq_85_right_knuckle_link" />
+            <child link="${prefix}robotiq_85_right_finger_link" />
+            <origin xyz="-0.03152616 0.0 -0.00376347" rpy="0 0 0" />
+        </joint>
+
+        <joint name="${prefix}left_inner_knuckle_joint" type="continuous">
+            <parent link="${prefix}robotiq_85_base_link" />
+            <child link="${prefix}robotiq_85_left_inner_knuckle_link" />
+            <axis xyz="0 -1 0" />
+            <origin xyz="0.0127 0.0 0.06142" rpy="0 0 0" />
+            <mimic joint="${prefix}finger_joint" />
+        </joint>
+
+        <joint name="${prefix}right_inner_knuckle_joint" type="continuous">
+            <parent link="${prefix}robotiq_85_base_link" />
+            <child link="${prefix}robotiq_85_right_inner_knuckle_link" />
+            <axis xyz="0 -1 0" />
+            <origin xyz="-0.0127 0.0 0.06142" rpy="0 0 0" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" />
+        </joint>
+
+        <joint name="${prefix}left_inner_finger_pad_joint" type="continuous">
+            <parent link="${prefix}robotiq_85_left_finger_link" />
+            <child link="${prefix}robotiq_85_left_finger_tip_link" />
+            <axis xyz="0 -1 0" />
+            <origin xyz="0.00563134 0.0 0.04718515" rpy="0 0 0" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" />
+        </joint>
+
+        <joint name="${prefix}right_inner_finger_pad_joint" type="continuous">
+            <parent link="${prefix}robotiq_85_right_finger_link" />
+            <child link="${prefix}robotiq_85_right_finger_tip_link" />
+            <axis xyz="0 -1 0" />
+            <origin xyz="-0.00563134 0.0 0.04718515" rpy="0 0 0" />
+            <mimic joint="${prefix}finger_joint" />
+        </joint>
+    </xacro:macro>
+</robot>

src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotiq_2f_interface)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+
+include_directories(include)
+
+# Declare a C++ library
+add_library(${PROJECT_NAME} SHARED
+  src/hardware_interface.cpp
+  src/Robotiq2fSocketAdapter.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  hardware_interface
+  pluginlib
+)
+
+pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml)
+
+# Install targets
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# Install header files
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+# Install plugin description file
+install(FILES
+  hardware_interface_plugin.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml

@@ -0,0 +1,9 @@
+<library path="robotiq_2f_interface">
+    <class name="robotiq_2f_interface/RobotiqGripperHardwareInterface"
+           type="robotiq_interface::RobotiqGripperHardwareInterface"
+           base_class_type="hardware_interface::SystemInterface">
+        <description>
+            ROS2 controller for the Robotiq gripper.
+        </description>
+    </class>
+</library>

src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp

@@ -0,0 +1,105 @@
+// MockRobotiq2fSocketAdapter.hpp
+
+#ifndef MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
+#define MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
+
+#include <string>
+#include <map>
+#include <atomic>
+#include <tuple>
+
+#include "robotiq_2f_interface/SocketAdapterBase.hpp"
+
+namespace robotiq_interface
+{
+class MockRobotiq2fSocketAdapter : public SocketAdapterBase {
+public:
+        MockRobotiq2fSocketAdapter() : isConnected(false), gripperPosition(0), gripperForce(0), gripperSpeed(0), isActive(false) {}
+
+    ~MockRobotiq2fSocketAdapter() override {
+        if (movementThread && movementThread->joinable()) {
+            movementThread->join();
+        }
+    }
+
+    bool connecting(const std::string& hostname, int port) override {
+        isConnected.store(true);
+        return true; // Simulate successful connection
+    }
+
+    void disconnect() override {
+        isConnected.store(false);
+    }
+
+    int force() override {
+        return gripperForce.load();
+    }
+
+    int speed() override {
+        return gripperSpeed.load();
+    }
+
+    int position() override {
+        return gripperPosition.load();
+    }
+
+    bool is_active() override {
+        return isActive.load();
+    }
+
+    void activate(bool autoCalibrate = true) override {
+        isActive.store(true);  // Ensure we set active before moving
+        if (!autoCalibrate) {
+            // Simulate a calibration sequence
+            move_and_wait(0, 255, 1);
+        }
+    }
+
+    void deactivate() override {
+        isActive.store(false);
+    }
+
+    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(); // Ensure no concurrent movements
+        }
+
+        gripperSpeed.store(speed);  // Simulate immediate speed update
+        gripperForce.store(force);  // Simulate immediate force update
+
+        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:
+    std::atomic<bool> isConnected;
+    std::atomic<int> gripperPosition;
+    std::atomic<int> gripperForce;
+    std::atomic<int> gripperSpeed;
+    std::atomic<bool> isActive;
+
+    std::unique_ptr<std::thread> movementThread;
+
+    void simulateMovement(int targetPosition, int speed, int force) {
+        int step = (targetPosition > gripperPosition.load()) ? 1 : -1;
+
+        // Simulate moving towards the target position
+        while (gripperPosition.load() != targetPosition) {
+            gripperPosition.fetch_add(step);
+            std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Simulate time to move a step
+        }
+
+        // std::cout << "Reached position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
+    }
+};
+} // end robotiq_interface
+
+#endif // MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP

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

@@ -0,0 +1,365 @@
+// 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>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "robotiq_2f_interface/SocketAdapterBase.hpp"
+
+namespace robotiq_interface
+{
+// 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 SocketAdapterBase {
+public:
+
+    Robotiq2fSocketAdapter() ;
+    ~Robotiq2fSocketAdapter() override;
+
+    // Connection management
+    /**
+     * Establishes a TCP connection to the Robotiq gripper.
+     *
+     * Attempts to open a socket and connect to the spec    ified 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 connecting(const std::string& hostname, int port) override;
+    /**
+    * 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() override;
+
+
+    /**
+    * 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, unsigned 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, unsigned 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
+    int force() override;
+    int speed() override;
+    int position() override;
+
+    /**
+    * 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() override;
+
+
+    // 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 = false) override;
+    /**
+    * 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 auto_calibrate(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() override;
+
+    // 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) override;
+    /**
+    * 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 socket_mutex_; // Mutex for socket access synchronization
+
+    // bounds of the encoded gripper states
+    int min_position_ = 3;
+    int max_position_ = 230;
+    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=2);
+
+
+    /**
+    * 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.
+    */
+    unsigned 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
+    const int MAX_RETRIES = 200;
+    const int RETRY_DELAY_MS = 50;
+
+    // Logging
+    const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqSocketAdapter");
+};
+} // end robotiq_interface
+
+#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP

src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp

@@ -0,0 +1,26 @@
+// SocketAdapterBase.hpp
+
+#ifndef SOCKET_ADAPTER_BASE_HPP
+#define SOCKET_ADAPTER_BASE_HPP
+
+#include <string>
+#include <tuple>
+
+namespace robotiq_interface
+{
+class SocketAdapterBase {
+public:
+    virtual ~SocketAdapterBase() = default;
+
+    virtual bool connecting(const std::string& hostname, int port) = 0;
+    virtual void disconnect() = 0;
+    virtual int force() = 0;
+    virtual int speed() = 0;
+    virtual int position() = 0;
+    virtual bool is_active() = 0;
+    virtual void activate(bool autoCalibrate = true) = 0;
+    virtual void deactivate() = 0;
+    virtual std::tuple<bool, int> move(int position, int speed, int force) = 0;
+};
+} // end robotiq_interface
+#endif // SOCKET_ADAPTER_BASE_HPP

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

@@ -0,0 +1,127 @@
+// hardware_interface.hpp
+
+#ifndef ROBOTIQ2F_HARDWARE_INTERFACE_HPP
+#define ROBOTIQ2F_HARDWARE_INTERFACE_HPP
+
+#include <chrono>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <vector>
+#include <atomic>
+
+#include "robotiq_2f_interface/visibility_control.hpp"
+
+#include "robotiq_2f_interface/SocketAdapterBase.hpp"
+#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
+#include "robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp"
+
+#include <hardware_interface/handle.hpp>
+#include <hardware_interface/hardware_info.hpp>
+#include <hardware_interface/system_interface.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+#include <hardware_interface/types/hardware_interface_return_values.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/clock.hpp>
+
+#include <stdexcept>
+
+namespace robotiq_interface
+{
+class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
+{
+public:
+    // Constructors
+    ROBOTIQ_INTERFACE_PUBLIC RobotiqGripperHardwareInterface();
+    ROBOTIQ_INTERFACE_PUBLIC ~RobotiqGripperHardwareInterface();
+    // ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr<SocketFactory> socket_factory);
+
+    // Lifecycle Management
+    ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
+    ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+    ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+    ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+    // State and Command Interfaces
+    ROBOTIQ_INTERFACE_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+    ROBOTIQ_INTERFACE_PUBLIC std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+    // Data Access (Read/Write)
+    ROBOTIQ_INTERFACE_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+    ROBOTIQ_INTERFACE_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
+    bool use_mock_hardware_;
+    std::unique_ptr<SocketAdapterBase> socket_adapter_;
+    void configureAdapter(bool useMock);
+
+    // Background communication thread
+    std::thread communication_thread_;
+    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();
+
+    // Conversion Methods
+    double convertRawToGap(int raw_position);
+    double convertRawToAngle(int raw_position);
+    unsigned int convertGapToRaw(double gap);
+    unsigned int convertSpeedToRaw(double speed);
+    unsigned 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_gap_ = 0.0;
+    double gripper_angle_ = 0.0;
+    double gripper_velocity_ = 0.0;
+    double gripper_gap_command_ = 0.0;
+
+    rclcpp::Clock::SharedPtr clock_ = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+    rclcpp::Time last_update_time_ = clock_ -> now();
+    rclcpp::Time current_time_ = clock_ -> now();
+
+    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 max_angle_;
+    double min_speed_;
+    double max_speed_;
+    double min_force_;
+    double max_force_;
+
+    // Last Command Values
+    int last_cmd_gap = 0;
+    int last_cmd_speed = 0;
+    int last_cmd_force = 0;
+    int last_raw_gap = 0;
+
+    // loop time
+    const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 2 };
+
+    // Logger
+    const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
+};
+} // end robotiq_interface
+
+#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP

src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp

@@ -0,0 +1,56 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_
+#define ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define ROBOTIQ_INTERFACE_EXPORT __attribute__((dllexport))
+#define ROBOTIQ_INTERFACE_IMPORT __attribute__((dllimport))
+#else
+#define ROBOTIQ_INTERFACE_EXPORT __declspec(dllexport)
+#define ROBOTIQ_INTERFACE_IMPORT __declspec(dllimport)
+#endif
+#ifdef ROBOTIQ_INTERFACE_BUILDING_DLL
+#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_EXPORT
+#else
+#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_IMPORT
+#endif
+#define ROBOTIQ_INTERFACE_PUBLIC_TYPE ROBOTIQ_INTERFACE_PUBLIC
+#define ROBOTIQ_INTERFACE_LOCAL
+#else
+#define ROBOTIQ_INTERFACE_EXPORT __attribute__((visibility("default")))
+#define ROBOTIQ_INTERFACE_IMPORT
+#if __GNUC__ >= 4
+#define ROBOTIQ_INTERFACE_PUBLIC __attribute__((visibility("default")))
+#define ROBOTIQ_INTERFACE_LOCAL __attribute__((visibility("hidden")))
+#else
+#define ROBOTIQ_INTERFACE_PUBLIC
+#define ROBOTIQ_INTERFACE_LOCAL
+#endif
+#define ROBOTIQ_INTERFACE_PUBLIC_TYPE
+#endif
+
+#endif  // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_

src/robotiq_2f/robotiq_2f_interface/package.xml

@@ -1,20 +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>servo_keyboard</name>
+  <name>robotiq_2f_interface</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
-  <maintainer email="ligerfotis@gmail.com">Fotios Lyegrakis</maintainer>
+  <maintainer email="root@todo.todo">root</maintainer>
-  <license>Apache-2.0</license>
+  <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <!-- Dependencies -->
   <depend>rclcpp</depend>
-  <depend>control_msgs</depend>
+  <depend>rclcpp_lifecycle</depend>
-  <depend>geometry_msgs</depend>
+  <depend>hardware_interface</depend>
-  <build_depend>controller_manager_msgs</build_depend>
+  <depend>pluginlib</depend>
-  <exec_depend>controller_manager_msgs</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp

@@ -0,0 +1,440 @@
+#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
+
+
+namespace robotiq_interface
+{
+Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
+}
+
+
+Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
+    disconnect();  // Ensure the socket is closed properly.
+}
+
+// Connection and disconnection
+bool Robotiq2fSocketAdapter::connecting(const std::string& hostname, int port) {
+    std::lock_guard<std::mutex> lock(socket_mutex_);
+    if (*sockfd_ >= 0) {
+        // std::cerr << "Already connected. Disconnecting to reconnect.\n";
+        RCLCPP_ERROR(logger_, "Already connected. Disconnecting to reconnect.");
+        disconnect();
+    }
+
+    int sock = socket(AF_INET, SOCK_STREAM, 0);
+    if (sock < 0) {
+        // std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
+        RCLCPP_ERROR(logger_, "Socket creation failed!");
+        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";
+        RCLCPP_ERROR(logger_, "Invalid address: %s", hostname.c_str());
+        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";
+        RCLCPP_ERROR(logger_, "Connection to  %s: %d failed!", hostname.c_str(), port);
+        close(*sockfd_);
+        *sockfd_ = -1;
+        return false;
+    }
+    else{
+        RCLCPP_INFO(logger_, "Connection to  %s: %d established!", hostname.c_str(), port);
+        return true;
+    }
+    RCLCPP_ERROR(logger_, "Unknown connection error in socket adapter connecting!");
+    return false;
+}
+
+void Robotiq2fSocketAdapter::disconnect() {
+    std::lock_guard<std::mutex> lock(socket_mutex_);
+    if (*sockfd_ >= 0) {
+        if (close(*sockfd_) == -1) {
+            // std::cerr << "Error closing socket: " << strerror(errno) << "\n";
+            RCLCPP_ERROR(logger_, "Error closing socket:  %s", strerror(errno));
+        } else {
+            // std::cout << "Disconnected successfully.\n";
+            RCLCPP_INFO(logger_, "Disconnected successfully.");
+        }
+        *sockfd_ = -1;
+    }
+}
+
+// Utility methods
+bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
+    std::lock_guard<std::mutex> lock(socket_mutex_);
+    if (*sockfd_ < 0) {
+        // std::cerr << "Attempted to send command on a disconnected socket.\n";
+        RCLCPP_ERROR(logger_, "Attempted to send command on a disconnected socket.");
+        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";
+        RCLCPP_ERROR(logger_, "Failed to send command: %s", strerror(errno));
+        return false;
+    }
+
+    return true;
+}
+
+std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
+    std::lock_guard<std::mutex> lock(socket_mutex_);
+    if (*sockfd_ < 0) {
+        // std::cerr << "Attempted to send command on a disconnected socket.\n";
+        RCLCPP_ERROR(logger_, "Attempted to send command on a disconnected socket.");
+        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);
+
+        select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
+/*
+        int activity = select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
+        if (activity == -1) {
+            // std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
+            RCLCPP_ERROR(logger_, "[receiveResponse] Error in select(): %s", strerror(errno));
+            return "";
+        } else if (activity == 0) {
+            RCLCPP_ERROR(logger_, "[receiveResponse] Activity is Zero!");
+            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";
+                RCLCPP_ERROR(logger_, "Failed to receive response: %s", strerror(errno));
+            }
+            return "";
+        } else if (bytes_read == 0) {
+            // std::cerr << "Connection closed by peer.\n";
+            RCLCPP_ERROR(logger_, "Connection closed by peer.");
+            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;
+            } else if (result == "ack"){
+                complete_response = true;
+            }
+        }
+    }
+    return result;
+}
+
+bool Robotiq2fSocketAdapter::isAck(const std::string& data) {
+    return data == ACK_MESSAGE;
+}
+
+unsigned int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
+    int clipped = std::max(min_value, std::min(value, max_value));
+    return static_cast<unsigned int>(clipped);
+}
+
+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, unsigned int>& variableMap) {
+    // Build the command string
+    std::string cmd = SET_COMMAND;
+    for (const auto& [variable, value] : variableMap) {
+        std::stringstream ss;
+        ss << value;
+        cmd += " " + variable + " " + ss.str();
+    }
+    cmd += "\n";
+
+    // Send the command and receive the response
+
+    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();
+    RCLCPP_DEBUG(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    return isAck(response);
+}
+
+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 += " " + variable + " " + ss.str();
+    }
+    cmd += "\n";
+
+    // Send the command and receive the response
+
+    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();
+    RCLCPP_DEBUG(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    return isAck(response);
+}
+
+bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, unsigned int value) {
+    std::stringstream ss;
+    ss << value;  // Convert the value to a string
+
+    std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n";
+
+    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();
+    RCLCPP_DEBUG(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    return isAck(response);
+}
+
+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 + " " + variable + " " + ss.str() + "\n";
+
+    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();
+    RCLCPP_DEBUG(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    return isAck(response);
+}
+
+int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
+    std::string cmd = GET_COMMAND + " " + variable + "\n";
+
+    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();
+    RCLCPP_DEBUG(logger_, "[getGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    std::istringstream iss(response);
+    std::string var_name, value_str;
+
+    if (response == ACK_MESSAGE) {
+        RCLCPP_DEBUG(logger_, "[getGripperVariable]Gripper still responding with ACK_MESSAGE");
+        return -1;
+    } else if (!(iss >> var_name >> value_str)) {
+        RCLCPP_ERROR(logger_, "Invalid gripper response format: %s for cmd: %s", response.c_str(), cmd.c_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;
+        RCLCPP_ERROR(logger_, "Failed to parse gripper response: %s",  ex.what());
+        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, static_cast<unsigned int>(1)); // Activate the gripper
+    waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active
+
+    if (!autoCalibrate) {
+        auto_calibrate();
+    }
+    RCLCPP_DEBUG(logger_, "Socket adapter activated.");
+}
+
+void Robotiq2fSocketAdapter::reset() {
+    setGripperVariable(CMD_ACT, static_cast<unsigned int>(0));
+    setGripperVariable(CMD_ATR, static_cast<unsigned int>(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) {
+    unsigned int clippedPosition = clip_val(min_position_, position, max_position_);
+    unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
+    unsigned int clippedForce = clip_val(min_force_, force, max_force_);
+
+    RCLCPP_DEBUG(logger_, "Current min/max positions: [%d, %d]", min_position_, max_position_);
+    RCLCPP_DEBUG(logger_, "Clipped command: %d", clippedPosition);
+
+    // Create a map for gripper variables (similar to Python's OrderedDict)
+    std::map<std::string, unsigned int> variableMap = {
+        { CMD_POS, clippedPosition },
+        { CMD_SPE, clippedSpeed },
+        { CMD_FOR, clippedForce },
+        { CMD_GTO, static_cast<unsigned int>(1) }
+    };
+    bool setResult = setGripperVariables(variableMap);
+    if (!setResult)
+    {
+        std::string result;
+        for (const auto& pair : variableMap) {
+            result += "{" + pair.first + ", " + std::to_string(pair.second) + "}, ";
+        }
+        RCLCPP_ERROR(logger_, "[move] failed for CMD Map: %s", result.c_str());
+    }
+    return std::make_tuple(setResult, clippedPosition);
+}
+
+std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
+    unsigned int clippedPosition = clip_val(min_position_, position, max_position_);
+    unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
+    unsigned int clippedForce = clip_val(min_force_, force, max_force_);
+
+    auto move_result = move(clippedPosition, clippedSpeed, clippedForce);
+    if (!std::get<0>(move_result)) {
+        // Handle the error case, e.g., throw an exception
+        RCLCPP_ERROR(logger_, "Failed to set variables for move.");
+    }
+
+    // Wait until position is acknowledged:
+    while(getGripperVariable(CMD_PRE) != std::get<1>(move_result)) {
+        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) {
+    // Open in case we are holding an object
+    RCLCPP_INFO(logger_, "Calibration started");
+    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";
+        RCLCPP_INFO(logger_, "Gripper auto-calibrated to [%d, %d]", min_position_, max_position_);
+    }
+}
+} // end robotiq_interface

src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp

@@ -0,0 +1,545 @@
+#include "robotiq_2f_interface/hardware_interface.hpp"
+
+namespace robotiq_interface
+{
+
+const std::string robot_ip_parameter_name = "robot_ip";
+const std::string robot_ip_default = "192.168.1.1";
+
+const std::string robot_port_parameter_name = "robot_port";
+const int robot_port_default = 63352;
+
+const std::string use_mock_hardware_parameter_name = "use_fake_hardware";
+const bool use_mock_hardware_default = true;
+
+const std::string min_position_parameter_name = "min_position";
+const double min_position_default = 0.01;
+
+const std::string max_position_parameter_name = "max_position";
+const double max_position_default = 0.0;
+
+const std::string max_angle_parameter_name = "max_angle";
+const double max_angle_default = 0.5;
+
+const std::string min_speed_parameter_name = "min_speed";
+const double min_speed_default = 0.0;
+
+const std::string max_speed_parameter_name = "max_speed";
+const double max_speed_default = 0.0;
+
+const std::string min_force_parameter_name = "min_force";
+const double min_force_default = 0.0;
+
+const std::string max_force_parameter_name = "max_force";
+const double max_force_default = 0.0;
+
+RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
+  : communication_thread_is_running_(false)
+{
+RCLCPP_DEBUG(logger_, "Constructor");
+}
+
+RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
+{
+    if (communication_thread_is_running_.load())
+    {
+        communication_thread_is_running_.store(false);
+        if (communication_thread_.joinable())
+        {
+            communication_thread_.join();
+        }
+    }
+}
+
+hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
+{
+    RCLCPP_DEBUG(logger_, "on_init");
+
+    if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+    {
+        return CallbackReturn::ERROR;
+    }
+
+    try
+    {
+        if (info.hardware_parameters.count(robot_ip_parameter_name) > 0) {
+            robot_ip_ = info.hardware_parameters.at(robot_ip_parameter_name);
+        } else {
+            RCLCPP_DEBUG(logger_, "Falling back on default ip!");
+            robot_ip_ = robot_ip_default;
+        }
+        RCLCPP_INFO(logger_, "Accessing parameter: Robot IP, Value: %s", robot_ip_.c_str());
+
+        if (info.hardware_parameters.count(robot_port_parameter_name) > 0) {
+            robot_port_ = std::stoi(info.hardware_parameters.at(robot_port_parameter_name));
+        } else {
+            robot_port_ = robot_port_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: Robot Port, Value: %d", robot_port_);
+
+        std::string param_value;
+        if (info.hardware_parameters.count(use_mock_hardware_parameter_name) > 0) {
+            param_value = info.hardware_parameters.at(use_mock_hardware_parameter_name);
+            use_mock_hardware_ = (param_value == "true" || param_value == "True");
+        } else {
+            use_mock_hardware_ = use_mock_hardware_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: use fake hardware , Value: %s", param_value.c_str());
+
+        if (info.hardware_parameters.count(min_position_parameter_name) > 0) {
+            min_position_ = std::stod(info.hardware_parameters.at(min_position_parameter_name));
+        } else {
+            min_position_ = min_position_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: min position, Value: %f", min_position_);
+
+        if (info.hardware_parameters.count(max_position_parameter_name) > 0) {
+            max_position_ = std::stod(info.hardware_parameters.at(max_position_parameter_name));
+        } else {
+            max_position_ = max_position_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: max position, Value: %f", max_position_);
+
+        if (info.hardware_parameters.count(max_angle_parameter_name) > 0) {
+            max_angle_ = std::stod(info.hardware_parameters.at(max_angle_parameter_name));
+        } else {
+            RCLCPP_WARN(logger_, "max angle not found falling back to default value!");
+            max_angle_ = max_angle_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: max angle , Value: %f", max_angle_);
+
+        if (info.hardware_parameters.count(min_speed_parameter_name) > 0) {
+            min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name));
+        } else {
+            min_speed_ = min_speed_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: min speed, Value: %f", min_speed_);
+
+        if (info.hardware_parameters.count(max_speed_parameter_name) > 0) {
+            max_speed_ = std::stod(info.hardware_parameters.at(max_speed_parameter_name));
+        } else {
+            max_speed_ = max_speed_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: max speed, Value: %f", max_speed_);
+
+                if (info.hardware_parameters.count(min_force_parameter_name) > 0) {
+            min_force_ = std::stod(info.hardware_parameters.at(min_force_parameter_name));
+        } else {
+            min_force_ = robot_port_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: min force, Value: %f", min_force_);
+
+        if (info.hardware_parameters.count(max_force_parameter_name) > 0) {
+            max_force_ = std::stod(info.hardware_parameters.at(max_force_parameter_name));
+        } else {
+            max_force_ = max_force_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: max force, Value: %f", max_force_);
+    }
+    catch (const std::exception& e)
+    {
+        RCLCPP_FATAL(logger_, "Failed to load parameters: %s", e.what());
+        return CallbackReturn::ERROR;
+    }
+
+    gripper_gap_ = std::numeric_limits<double>::quiet_NaN();
+    gripper_angle_ = std::numeric_limits<double>::quiet_NaN();
+    gripper_velocity_ = std::numeric_limits<double>::quiet_NaN();
+    gripper_gap_command_ = std::numeric_limits<double>::quiet_NaN();
+    reactivate_gripper_cmd_ = NO_NEW_CMD_;
+    reactivate_gripper_async_cmd_.store(false);
+
+    const hardware_interface::ComponentInfo& joint_gap = info_.joints[0];
+    const hardware_interface::ComponentInfo& joint_angle = info_.joints[1];
+
+    // There is one command interface: position.
+    if (joint_gap.command_interfaces.size() != 1)
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint_gap.name.c_str(),
+                     joint_gap.command_interfaces.size());
+        return CallbackReturn::ERROR;
+    }
+
+    if (joint_gap.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint_gap.name.c_str(),
+                     joint_gap.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+        return CallbackReturn::ERROR;
+    }
+
+    // There are two state interfaces for the gripper gap: position and velocity.
+    if (joint_gap.state_interfaces.size() != 2)
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint_gap.name.c_str(),
+                     joint_gap.state_interfaces.size());
+        return CallbackReturn::ERROR;
+    }
+
+    for (int i = 0; i < 2; ++i)
+    {
+        if (!(joint_gap.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
+              joint_gap.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
+        {
+            RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint_gap.name.c_str(),
+                       joint_gap.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
+                       hardware_interface::HW_IF_VELOCITY);
+            return CallbackReturn::ERROR;
+        }
+    }
+
+        // There are two state interfaces for the gripper angle: position.
+    if (joint_angle.state_interfaces.size() != 1)
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 1 expected.", joint_angle.name.c_str(),
+                     joint_angle.state_interfaces.size());
+        return CallbackReturn::ERROR;
+    }
+
+    if (!(joint_angle.state_interfaces[0].name == hardware_interface::HW_IF_POSITION))
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint_angle.name.c_str(),
+                   joint_angle.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION,
+                   hardware_interface::HW_IF_VELOCITY);
+        return CallbackReturn::ERROR;
+    }
+
+
+
+    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;
+        }
+
+        configureAdapter(use_mock_hardware_);
+    }
+    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_gap_))
+    {
+        gripper_gap_ = 0.0;
+        gripper_angle_ = 0.0;
+        gripper_velocity_ = 0.0;
+        gripper_gap_command_ = 0.0;
+    }
+
+    // Activate the gripper.
+    try
+    {
+        RCLCPP_INFO(logger_, "Starting Socket connection!");
+        if (!socket_adapter_->connecting(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;
+        }
+        socket_adapter_->deactivate();
+        socket_adapter_->activate();
+
+        communication_thread_is_running_.store(true);
+        communication_thread_ = std::thread([this] { this->background_task(); });
+        RCLCPP_INFO(logger_, "Background task thread started.");
+    }
+    catch (const std::exception& e)
+    {
+        RCLCPP_FATAL(logger_, "Failed to communicate with the Robotiq gripper: %s", e.what());
+        return CallbackReturn::ERROR;
+    }
+
+    if (!socket_adapter_->is_active()) {
+        RCLCPP_ERROR(logger_, "Attempted to activate while gripper is not active!");
+        return CallbackReturn::ERROR;
+    }
+    else{
+        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_gap_));
+    state_interfaces.emplace_back(
+        hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_));
+    state_interfaces.emplace_back(
+        hardware_interface::StateInterface(info_.joints[1].name, hardware_interface::HW_IF_POSITION, &gripper_angle_));
+    for (const auto& interface : state_interfaces) {
+    RCLCPP_DEBUG(logger_, "Exporting state interface for joint: %s type: %s",
+                interface.get_name().c_str(), interface.get_interface_name().c_str());
+    }
+
+    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_gap_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, "max_velocity", &gripper_speed_));
+    command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(info_.joints[0].name, "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_));
+
+    for (const auto& interface : command_interfaces) {
+    RCLCPP_DEBUG(logger_, "Exporting command interface for joint: %s type: %s",
+                interface.get_name().c_str(), interface.get_interface_name().c_str());
+}
+
+    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");
+
+    // Ensure that the node uses the appropriate clock source
+
+    current_time_ = clock_->now();
+
+    try {
+        int raw_position = socket_adapter_->position();
+        double new_gap = convertRawToGap(raw_position);
+        double new_angle = convertRawToAngle(raw_position);
+
+        if (!std::isnan(gripper_gap_)) {
+            double time_difference = (current_time_ - last_update_time_).seconds();
+            if (time_difference > 0) {
+                gripper_velocity_ = (new_gap - gripper_gap_) / time_difference;
+            }
+        }
+
+        gripper_gap_ = new_gap;
+        gripper_angle_ = new_angle;
+        last_update_time_ = current_time_;
+
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(logger_, "Failed to read from Robotiq gripper: %s", e.what());
+        return hardware_interface::return_type::ERROR;
+    }
+
+    // Process asynchronous reactivation response if available
+    if (!std::isnan(reactivate_gripper_cmd_))
+    {
+        RCLCPP_INFO(logger_, "Sending gripper reactivation request.");
+        reactivate_gripper_async_cmd_.store(true);
+        reactivate_gripper_cmd_ = NO_NEW_CMD_;
+    }
+
+    if (reactivate_gripper_async_response_.load().has_value())
+    {
+        reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value();
+        reactivate_gripper_async_response_.store(std::nullopt);
+    }
+
+    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_gap = convertGapToRaw(gripper_gap_command_);
+        int scaled_speed = convertSpeedToRaw(gripper_speed_);
+        int scaled_force = convertForceToRaw(gripper_force_);
+
+        // Store the scaled values into atomic variables
+        write_command_.store(scaled_gap);
+        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())
+    {
+        std::lock_guard<std::mutex> guard(resource_mutex_);
+        if (reactivate_gripper_async_cmd_.load())
+        {
+            try
+            {
+                // Execute the reactivation sequence
+                RCLCPP_INFO(logger_, "Reactivating gripper.");
+                socket_adapter_->deactivate();
+                socket_adapter_->activate(false);
+                reactivate_gripper_async_cmd_.store(false);  // Reset the command flag
+                reactivate_gripper_async_response_.store(true);  // Store the success of the operation
+            }
+            catch (const std::exception& e)
+            {
+                RCLCPP_ERROR(logger_, "Failed to reactivate gripper: %s", e.what());
+                reactivate_gripper_async_response_.store(false);  // Store the failure of the operation
+            }
+        }
+
+        // Additional periodic operations
+        performRegularOperations();
+        std::this_thread::sleep_for(gripperCommsLoopPeriod);  // Sleep for the duration of the communication loop period
+    }
+}
+
+void RobotiqGripperHardwareInterface::performRegularOperations()
+{
+    try
+    {
+        int scaled_gap = write_command_.load();
+        int scaled_speed = write_speed_.load();
+        int scaled_force = write_force_.load();
+
+        if (scaled_gap != last_cmd_gap || scaled_speed != last_cmd_speed || scaled_force != last_cmd_force){
+            RCLCPP_DEBUG(logger_, "New move cmd: POS: %d, SPE: %d, FOR: %d", scaled_gap, scaled_speed , scaled_force);
+            auto result = socket_adapter_->move(scaled_gap, scaled_speed, scaled_force);
+            if (!std::get<0>(result)) {
+                throw std::runtime_error("Failed to send move command to Robotiq gripper.");
+            }
+            last_cmd_gap = scaled_gap;
+            last_cmd_speed = scaled_speed;
+            last_cmd_force = scaled_force;
+        }
+        int raw_gap = socket_adapter_->position();
+        gripper_current_state_.store(convertRawToGap(raw_gap));
+    }
+    catch (const std::exception& e)
+    {
+        RCLCPP_ERROR(logger_, "Regular operation error: %s", e.what());
+    }
+}
+
+
+// Mock Hardware
+void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) {
+    if (useMock) {
+        RCLCPP_INFO(logger_, "Using Mock Robotiq Adapter");
+        socket_adapter_ = std::make_unique<MockRobotiq2fSocketAdapter>();
+    } else {
+        RCLCPP_INFO(logger_, "Using Real Robotiq Adapter");
+        socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
+    }
+}
+
+
+// Conversion methods
+double RobotiqGripperHardwareInterface::convertRawToGap(int raw_position) {
+    if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
+        throw std::runtime_error("Invalid gripper position limits.");
+    }
+    double scaled_position = min_position_ + ((230 - static_cast<double>(raw_position)) / 227.0) * (max_position_ - min_position_);
+    return std::max(min_position_, std::min(scaled_position, max_position_));
+}
+
+double RobotiqGripperHardwareInterface::convertRawToAngle(int raw_position) {
+    if (std::isnan(max_angle_) || 0.0 >= max_angle_) {
+        throw std::runtime_error("Invalid gripper angle limits: max_angle is not configured correctly.");
+    }
+    double scaled_angle = ((230 - static_cast<double>(raw_position)) / 227.0) * max_angle_;
+    return std::max(0.0, std::min(scaled_angle, max_angle_));
+}
+
+unsigned int RobotiqGripperHardwareInterface::convertGapToRaw(double gap) {
+    if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
+        throw std::runtime_error("Invalid gripper position limits.");
+    }
+    unsigned int raw = static_cast<unsigned int>(230 - ((gap - min_position_) / (max_position_ - min_position_)) * 227);
+    return std::clamp(raw, 3u, 230u); // Ensuring the output is within the valid range
+}
+
+unsigned int 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<unsigned int>(std::clamp(scaled, 0.0, 255.0));
+}
+
+unsigned int 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<unsigned int>(std::clamp(scaled, 0.0, 255.0));
+}
+
+}  // namespace robotiq_interface
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(robotiq_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();
+}

src/robotiq_2f/robotiq_2f_msgs/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotiq_2f_msgs)
+
+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(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(action_msgs REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ForwardCommand.msg"
+  "action/GripperCommand.action"
+  # Add more messages here
+  DEPENDENCIES builtin_interfaces action_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action

@@ -0,0 +1,12 @@
+# Goal definition
+float64 desired_position    # in m
+float64 max_velocity        # in m/s
+float64 max_effort          # in Newtons
+---
+# Result definition
+float64 final_position      # The final gripper gap size (in meters)
+bool stalled                # True if the gripper is exerting max effort and not moving
+bool reached_goal           # True if the gripper position has reached the commanded setpoint
+---
+# Feedback definition
+float64 current_position    # in m

src/robotiq_2f/robotiq_2f_msgs/msg/ForwardCommand.msg

@@ -0,0 +1,3 @@
+float64 position
+float64 max_velocity
+float64 max_effort

src/robotiq_2f/robotiq_2f_msgs/package.xml

@@ -0,0 +1,26 @@
+<?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_msgs</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>
+
+  <depend>std_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>action_msgs</depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2_robotiq_gripper

@@ -1 +0,0 @@
-Subproject commit b6136bdc866a929bfb096890ca61dde1335ffd30

src/serial

@@ -1 +0,0 @@
-Subproject commit d8d160678aa0b31cdf467c052b954fa287cc6cdf

src/servo_keyboard/CMakeLists.txt

@@ -1,67 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(servo_keyboard)
-
-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(geometry_msgs REQUIRED)
-find_package(control_msgs REQUIRED)
-find_package(controller_manager_msgs REQUIRED)
-find_package(yaml-cpp REQUIRED)
-find_package(ament_index_cpp REQUIRED)
-
-
-add_executable(servo_keyboard_input src/servo_keyboard_input.cpp)
-target_include_directories(servo_keyboard_input PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
-target_compile_features(servo_keyboard_input PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-
-# Include directories
-target_include_directories(servo_keyboard_input PRIVATE
-  ${rclcpp_INCLUDE_DIRS}
-  ${geometry_msgs_INCLUDE_DIRS}
-  ${control_msgs_INCLUDE_DIRS}
-  ${controller_manager_msgs_INCLUDE_DIRS}
-  ${YAML_CPP_INCLUDE_DIRS}  # Add this line to include YAML-cpp headers
-)
-
-# Link the executable with required libraries
-ament_target_dependencies(servo_keyboard_input
-  rclcpp
-  geometry_msgs
-  control_msgs
-  controller_manager_msgs
-  yaml-cpp  # Add this line to link against YAML-cpp
-    ament_index_cpp
-)
-target_link_libraries(servo_keyboard_input ${YAML_CPP_LIBRARIES})  # Add this line to link against YAML-cpp
-
-# Existing install directive for the executable
-install(TARGETS servo_keyboard_input
-  DESTINATION lib/${PROJECT_NAME})
-
-# Added install directive for the launch files
-install(DIRECTORY
-  launch
-  DESTINATION share/${PROJECT_NAME}/
-)
-
-
-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/servo_keyboard/LICENSE

@@ -1,202 +0,0 @@
-
-                                 Apache License
-                           Version 2.0, January 2004
-                        http://www.apache.org/licenses/
-
-   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
-
-   1. Definitions.
-
-      "License" shall mean the terms and conditions for use, reproduction,
-      and distribution as defined by Sections 1 through 9 of this document.
-
-      "Licensor" shall mean the copyright owner or entity authorized by
-      the copyright owner that is granting the License.
-
-      "Legal Entity" shall mean the union of the acting entity and all
-      other entities that control, are controlled by, or are under common
-      control with that entity. For the purposes of this definition,
-      "control" means (i) the power, direct or indirect, to cause the
-      direction or management of such entity, whether by contract or
-      otherwise, or (ii) ownership of fifty percent (50%) or more of the
-      outstanding shares, or (iii) beneficial ownership of such entity.
-
-      "You" (or "Your") shall mean an individual or Legal Entity
-      exercising permissions granted by this License.
-
-      "Source" form shall mean the preferred form for making modifications,
-      including but not limited to software source code, documentation
-      source, and configuration files.
-
-      "Object" form shall mean any form resulting from mechanical
-      transformation or translation of a Source form, including but
-      not limited to compiled object code, generated documentation,
-      and conversions to other media types.
-
-      "Work" shall mean the work of authorship, whether in Source or
-      Object form, made available under the License, as indicated by a
-      copyright notice that is included in or attached to the work
-      (an example is provided in the Appendix below).
-
-      "Derivative Works" shall mean any work, whether in Source or Object
-      form, that is based on (or derived from) the Work and for which the
-      editorial revisions, annotations, elaborations, or other modifications
-      represent, as a whole, an original work of authorship. For the purposes
-      of this License, Derivative Works shall not include works that remain
-      separable from, or merely link (or bind by name) to the interfaces of,
-      the Work and Derivative Works thereof.
-
-      "Contribution" shall mean any work of authorship, including
-      the original version of the Work and any modifications or additions
-      to that Work or Derivative Works thereof, that is intentionally
-      submitted to Licensor for inclusion in the Work by the copyright owner
-      or by an individual or Legal Entity authorized to submit on behalf of
-      the copyright owner. For the purposes of this definition, "submitted"
-      means any form of electronic, verbal, or written communication sent
-      to the Licensor or its representatives, including but not limited to
-      communication on electronic mailing lists, source code control systems,
-      and issue tracking systems that are managed by, or on behalf of, the
-      Licensor for the purpose of discussing and improving the Work, but
-      excluding communication that is conspicuously marked or otherwise
-      designated in writing by the copyright owner as "Not a Contribution."
-
-      "Contributor" shall mean Licensor and any individual or Legal Entity
-      on behalf of whom a Contribution has been received by Licensor and
-      subsequently incorporated within the Work.
-
-   2. Grant of Copyright License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      copyright license to reproduce, prepare Derivative Works of,
-      publicly display, publicly perform, sublicense, and distribute the
-      Work and such Derivative Works in Source or Object form.
-
-   3. Grant of Patent License. Subject to the terms and conditions of
-      this License, each Contributor hereby grants to You a perpetual,
-      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
-      (except as stated in this section) patent license to make, have made,
-      use, offer to sell, sell, import, and otherwise transfer the Work,
-      where such license applies only to those patent claims licensable
-      by such Contributor that are necessarily infringed by their
-      Contribution(s) alone or by combination of their Contribution(s)
-      with the Work to which such Contribution(s) was submitted. If You
-      institute patent litigation against any entity (including a
-      cross-claim or counterclaim in a lawsuit) alleging that the Work
-      or a Contribution incorporated within the Work constitutes direct
-      or contributory patent infringement, then any patent licenses
-      granted to You under this License for that Work shall terminate
-      as of the date such litigation is filed.
-
-   4. Redistribution. You may reproduce and distribute copies of the
-      Work or Derivative Works thereof in any medium, with or without
-      modifications, and in Source or Object form, provided that You
-      meet the following conditions:
-
-      (a) You must give any other recipients of the Work or
-          Derivative Works a copy of this License; and
-
-      (b) You must cause any modified files to carry prominent notices
-          stating that You changed the files; and
-
-      (c) You must retain, in the Source form of any Derivative Works
-          that You distribute, all copyright, patent, trademark, and
-          attribution notices from the Source form of the Work,
-          excluding those notices that do not pertain to any part of
-          the Derivative Works; and
-
-      (d) If the Work includes a "NOTICE" text file as part of its
-          distribution, then any Derivative Works that You distribute must
-          include a readable copy of the attribution notices contained
-          within such NOTICE file, excluding those notices that do not
-          pertain to any part of the Derivative Works, in at least one
-          of the following places: within a NOTICE text file distributed
-          as part of the Derivative Works; within the Source form or
-          documentation, if provided along with the Derivative Works; or,
-          within a display generated by the Derivative Works, if and
-          wherever such third-party notices normally appear. The contents
-          of the NOTICE file are for informational purposes only and
-          do not modify the License. You may add Your own attribution
-          notices within Derivative Works that You distribute, alongside
-          or as an addendum to the NOTICE text from the Work, provided
-          that such additional attribution notices cannot be construed
-          as modifying the License.
-
-      You may add Your own copyright statement to Your modifications and
-      may provide additional or different license terms and conditions
-      for use, reproduction, or distribution of Your modifications, or
-      for any such Derivative Works as a whole, provided Your use,
-      reproduction, and distribution of the Work otherwise complies with
-      the conditions stated in this License.
-
-   5. Submission of Contributions. Unless You explicitly state otherwise,
-      any Contribution intentionally submitted for inclusion in the Work
-      by You to the Licensor shall be under the terms and conditions of
-      this License, without any additional terms or conditions.
-      Notwithstanding the above, nothing herein shall supersede or modify
-      the terms of any separate license agreement you may have executed
-      with Licensor regarding such Contributions.
-
-   6. Trademarks. This License does not grant permission to use the trade
-      names, trademarks, service marks, or product names of the Licensor,
-      except as required for reasonable and customary use in describing the
-      origin of the Work and reproducing the content of the NOTICE file.
-
-   7. Disclaimer of Warranty. Unless required by applicable law or
-      agreed to in writing, Licensor provides the Work (and each
-      Contributor provides its Contributions) on an "AS IS" BASIS,
-      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
-      implied, including, without limitation, any warranties or conditions
-      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
-      PARTICULAR PURPOSE. You are solely responsible for determining the
-      appropriateness of using or redistributing the Work and assume any
-      risks associated with Your exercise of permissions under this License.
-
-   8. Limitation of Liability. In no event and under no legal theory,
-      whether in tort (including negligence), contract, or otherwise,
-      unless required by applicable law (such as deliberate and grossly
-      negligent acts) or agreed to in writing, shall any Contributor be
-      liable to You for damages, including any direct, indirect, special,
-      incidental, or consequential damages of any character arising as a
-      result of this License or out of the use or inability to use the
-      Work (including but not limited to damages for loss of goodwill,
-      work stoppage, computer failure or malfunction, or any and all
-      other commercial damages or losses), even if such Contributor
-      has been advised of the possibility of such damages.
-
-   9. Accepting Warranty or Additional Liability. While redistributing
-      the Work or Derivative Works thereof, You may choose to offer,
-      and charge a fee for, acceptance of support, warranty, indemnity,
-      or other liability obligations and/or rights consistent with this
-      License. However, in accepting such obligations, You may act only
-      on Your own behalf and on Your sole responsibility, not on behalf
-      of any other Contributor, and only if You agree to indemnify,
-      defend, and hold each Contributor harmless for any liability
-      incurred by, or claims asserted against, such Contributor by reason
-      of your accepting any such warranty or additional liability.
-
-   END OF TERMS AND CONDITIONS
-
-   APPENDIX: How to apply the Apache License to your work.
-
-      To apply the Apache License to your work, attach the following
-      boilerplate notice, with the fields enclosed by brackets "[]"
-      replaced with your own identifying information. (Don't include
-      the brackets!)  The text should be enclosed in the appropriate
-      comment syntax for the file format. We also recommend that a
-      file or class name and description of purpose be included on the
-      same "printed page" as the copyright notice for easier
-      identification within third-party archives.
-
-   Copyright [yyyy] [name of copyright owner]
-
-   Licensed under the Apache License, Version 2.0 (the "License");
-   you may not use this file except in compliance with the License.
-   You may obtain a copy of the License at
-
-       http://www.apache.org/licenses/LICENSE-2.0
-
-   Unless required by applicable law or agreed to in writing, software
-   distributed under the License is distributed on an "AS IS" BASIS,
-   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-   See the License for the specific language governing permissions and
-   limitations under the License.

src/servo_keyboard/config/servo_keyboard_params.yaml

@@ -1,6 +0,0 @@
-/keyboard_servo:
-  ros__parameters:
-    joint_vel_cmd: 1.0
-    gripper_speed_multiplier: 0.02
-    gripper_lower_limit: 0.0
-    gripper_upper_limit: 0.7

src/servo_keyboard/launch/servo_keyboard.launch.py

@@ -1,16 +0,0 @@
-from launch import LaunchDescription
-from launch_ros.actions import Node
-from ament_index_python.packages import get_package_share_directory
-import os
-def generate_launch_description():
-    config_path = os.path.join(get_package_share_directory('servo_keyboard'), 'config', 'servo_keyboard_params.yaml')
-
-    return LaunchDescription([
-        Node(
-            package='servo_keyboard',
-            executable='servo_keyboard_input',
-            name='servo_keyboard_input',
-            output='screen',
-            parameters=[config_path],
-        )
-    ])

src/servo_keyboard/src/servo_keyboard_input.cpp

@@ -1,423 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- *  Copyright (c) 2021, PickNik LLC
- *  All rights reserved.
- *
- *  Redistribution and use in source and binary forms, with or without
- *  modification, are permitted provided that the following conditions
- *  are met:
- *
- *   * Redistributions of source code must retain the above copyright
- *     notice, this list of conditions and the following disclaimer.
- *   * Redistributions in binary form must reproduce the above
- *     copyright notice, this list of conditions and the following
- *     disclaimer in the documentation and/or other materials provided
- *     with the distribution.
- *   * Neither the name of PickNik LLC nor the names of its
- *     contributors may be used to endorse or promote products derived
- *     from this software without specific prior written permission.
- *
- *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- *  POSSIBILITY OF SUCH DAMAGE.
- *********************************************************************/
-
-/*      Title     : servo_keyboard_input.cpp
- *      Project   : moveit2_tutorials
- *      Created   : 05/31/2021
- *      Author    : Adam Pettinger
- */
-
-#include <rclcpp/rclcpp.hpp>
-#include <geometry_msgs/msg/twist_stamped.hpp>
-#include <control_msgs/msg/joint_jog.hpp>
-#include <controller_manager_msgs/srv/switch_controller.hpp> // Add necessary includes
-#include <std_msgs/msg/float64_multi_array.hpp> // Add necessary includes
-#include <sensor_msgs/msg/joint_state.hpp> // Add necessary include for JointState message
-#include <ament_index_cpp/get_package_share_directory.hpp>
-
-#include <signal.h>
-#include <stdio.h>
-#include <termios.h>
-#include <unistd.h>
-
-// Define used keys
-#define KEYCODE_LEFT 0x44
-#define KEYCODE_RIGHT 0x43
-#define KEYCODE_UP 0x41
-#define KEYCODE_DOWN 0x42
-#define KEYCODE_PERIOD 0x2E
-#define KEYCODE_SEMICOLON 0x3B
-#define KEYCODE_E 0x65
-#define KEYCODE_W 0x77
-#define KEYCODE_1 0x31
-#define KEYCODE_2 0x32
-#define KEYCODE_3 0x33
-#define KEYCODE_4 0x34
-#define KEYCODE_5 0x35
-#define KEYCODE_6 0x36
-#define KEYCODE_R 0x72
-#define KEYCODE_Q 0x71
-#define KEYCODE_PLUS 0x2B   // Keycode for the plus sign (+)
-#define KEYCODE_MINUS 0x2D  // Keycode for the minus sign (-)
-#define KEYCODE_GRIPPER 0x67 // Keycode for the gripper control button (g)
-
-// Some constants used in the Servo Teleop demo
-const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
-const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
-const std::string GRIPPER_TOPIC = "/forward_gripper_position_controller/commands";
-const size_t ROS_QUEUE_SIZE = 10;
-const std::string EEF_FRAME_ID = "world";
-const std::string BASE_FRAME_ID = "tool0";
-
-// A class for reading the key inputs from the terminal
-class KeyboardReader
-{
-public:
-  KeyboardReader() : kfd(0)
-  {
-    // get the console in raw mode
-    tcgetattr(kfd, &cooked);
-    struct termios raw;
-    memcpy(&raw, &cooked, sizeof(struct termios));
-    raw.c_lflag &= ~(ICANON | ECHO);
-    // Setting a new line, then end of file
-    raw.c_cc[VEOL] = 1;
-    raw.c_cc[VEOF] = 2;
-    tcsetattr(kfd, TCSANOW, &raw);
-  }
-  void readOne(char* c)
-  {
-    int rc = read(kfd, c, 1);
-    if (rc < 0)
-    {
-      throw std::runtime_error("read failed");
-    }
-  }
-  void shutdown()
-  {
-    tcsetattr(kfd, TCSANOW, &cooked);
-  }
-
-private:
-  int kfd;
-  struct termios cooked;
-};
-
-// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller
-class KeyboardServo
-{
-public:
-  KeyboardServo();
-  int keyLoop();
-
-private:
-  void spin();
-
-  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_;
-
-  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
-  rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
-  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_pub_;
-  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
-
-  std::string frame_to_publish_;
-  double joint_vel_cmd_;
-  double gripper_speed_multiplier_;
-  double last_finger_joint_angle_;
-  double gripper_lower_limit_;
-  double gripper_upper_limit_;
-};
-
-KeyboardServo::KeyboardServo() {
-
-    // Create the node
-    nh_ = rclcpp::Node::make_shared("keyboard_servo");
-
-    // Declare the parameters
-    nh_->declare_parameter("joint_vel_cmd", 1.0);
-    nh_->declare_parameter("gripper_speed_multiplier", 0.1);
-    nh_->declare_parameter("gripper_lower_limit", 0.1);
-    nh_->declare_parameter("gripper_upper_limit", 0.4);
-
-    // Get the parameters from the node
-    nh_->get_parameter("joint_vel_cmd", joint_vel_cmd_);
-    nh_->get_parameter("gripper_speed_multiplier", gripper_speed_multiplier_);
-    nh_->get_parameter("gripper_lower_limit", gripper_lower_limit_);
-    nh_->get_parameter("gripper_upper_limit", gripper_upper_limit_);
-
-    // Print the parameters
-    printf("Joint velocity command: %f\n", joint_vel_cmd_);
-    printf("Gripper speed multiplier: %f\n", gripper_speed_multiplier_);
-    printf("Gripper lower limit: %f\n", gripper_lower_limit_);
-    printf("Gripper upper limit: %f\n", gripper_upper_limit_);
-
-    twist_pub_ = nh_->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, ROS_QUEUE_SIZE);
-    joint_pub_ = nh_->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE);
-    gripper_cmd_pub_ = nh_->create_publisher<std_msgs::msg::Float64MultiArray>(GRIPPER_TOPIC, ROS_QUEUE_SIZE);
-    joint_state_sub_ = nh_->create_subscription<sensor_msgs::msg::JointState>(
-    "/joint_states", ROS_QUEUE_SIZE,
-    std::bind(&KeyboardServo::jointStateCallback, this, std::placeholders::_1));
-
-}
-
-
-KeyboardReader input;
-
-// Implement the jointStateCallback function
-void KeyboardServo::jointStateCallback(sensor_msgs::msg::JointState::SharedPtr msg)
-{
-  // 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)
-{
-  auto msg = std::make_unique<std_msgs::msg::Float64MultiArray>();
-  msg->data.push_back(finger_joint_angle);
-  gripper_cmd_pub_->publish(std::move(msg));
-}
-
-void quit(int sig)
-{
-  (void)sig;
-  input.shutdown();
-  rclcpp::shutdown();
-  exit(0);
-}
-
-int main(int argc, char** argv)
-{
-  rclcpp::init(argc, argv);
-  KeyboardServo keyboard_servo;
-
-  signal(SIGINT, quit);
-
-  int rc = keyboard_servo.keyLoop();
-  input.shutdown();
-  rclcpp::shutdown();
-
-  return rc;
-}
-
-void KeyboardServo::spin()
-{
-  while (rclcpp::ok())
-  {
-    rclcpp::spin_some(nh_);
-  }
-}
-
-void KeyboardServo::handlePlusPress()
-{
-    // Calculate the new finger joint angle
-    double delta_angle = 1.0 * gripper_speed_multiplier_;
-    double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle;
-//    printf("New finger joint angle: %f\n", new_finger_joint_angle);
-    // Check if the new angle is within the limits
-    if (new_finger_joint_angle <= gripper_upper_limit_)
-    {
-        // Update the finger joint angle
-        last_finger_joint_angle_ = new_finger_joint_angle;
-//        printf("New finger joint angle: %f\n", last_finger_joint_angle_);
-        // Publish the gripper command with the new angle
-        publishGripperCommand(last_finger_joint_angle_);
-    }
-}
-
-void KeyboardServo::handleMinusPress()
-{
-    // Calculate the new finger joint angle
-    double delta_angle = -1.0 * gripper_speed_multiplier_;
-    double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle;
-
-    // Check if the new angle is within the limits
-    if (new_finger_joint_angle >= gripper_lower_limit_)
-    {
-        // Update the finger joint angle
-        last_finger_joint_angle_ = new_finger_joint_angle;
-
-        // Publish the gripper command with the new angle
-        publishGripperCommand(last_finger_joint_angle_);
-    }
-}
-
-
-
-int KeyboardServo::keyLoop()
-{
-  char c;
-  bool publish_twist = false;
-  bool publish_joint = false;
-
-  std::thread{ std::bind(&KeyboardServo::spin, this) }.detach();
-
-    puts("Reading from keyboard");
-    puts("---------------------------");
-    puts("Use arrow keys and the '.' and ';' keys to Cartesian jog");
-    puts("Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame");
-    puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging.");
-    puts("Use '+' to open the gripper, '-' to close it.");
-    puts("'Q' to quit.");
-
-
-  for (;;)
-  {
-    // get the next event from the keyboard
-    try
-    {
-      input.readOne(&c);
-    }
-    catch (const std::runtime_error&)
-    {
-      perror("read():");
-      return -1;
-    }
-
-    RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c);
-
-    // // Create the messages we might publish
-    auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
-    auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
-
-    // Use read key-press
-    switch (c)
-    {
-      case KEYCODE_LEFT:
-        RCLCPP_DEBUG(nh_->get_logger(), "LEFT");
-        twist_msg->twist.linear.y = -1.0;
-        publish_twist = true;
-        break;
-      case KEYCODE_RIGHT:
-        RCLCPP_DEBUG(nh_->get_logger(), "RIGHT");
-        twist_msg->twist.linear.y = 1.0;
-        publish_twist = true;
-        break;
-      case KEYCODE_UP:
-        RCLCPP_DEBUG(nh_->get_logger(), "UP");
-        twist_msg->twist.linear.x = 1.0;
-        publish_twist = true;
-        break;
-      case KEYCODE_DOWN:
-        RCLCPP_DEBUG(nh_->get_logger(), "DOWN");
-        twist_msg->twist.linear.x = -1.0;
-        publish_twist = true;
-        break;
-      case KEYCODE_PERIOD:
-        RCLCPP_DEBUG(nh_->get_logger(), "PERIOD");
-        twist_msg->twist.linear.z = -1.0;
-        publish_twist = true;
-        break;
-      case KEYCODE_SEMICOLON:
-        RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON");
-        twist_msg->twist.linear.z = 1.0;
-        publish_twist = true;
-        break;
-      case KEYCODE_E:
-        RCLCPP_DEBUG(nh_->get_logger(), "E");
-        frame_to_publish_ = EEF_FRAME_ID;
-        break;
-      case KEYCODE_W:
-        RCLCPP_DEBUG(nh_->get_logger(), "W");
-        frame_to_publish_ = BASE_FRAME_ID;
-        break;
-      case KEYCODE_1:
-        RCLCPP_DEBUG(nh_->get_logger(), "1");
-        joint_msg->joint_names.push_back("shoulder_lift_joint");
-        joint_msg->velocities.push_back(joint_vel_cmd_);
-        publish_joint = true;
-        break;
-      case KEYCODE_2:
-        RCLCPP_DEBUG(nh_->get_logger(), "2");
-        joint_msg->joint_names.push_back("shoulder_pan_joint");
-        joint_msg->velocities.push_back(joint_vel_cmd_);
-        publish_joint = true;
-        break;
-      case KEYCODE_3:
-        RCLCPP_DEBUG(nh_->get_logger(), "3");
-        joint_msg->joint_names.push_back("elbow_joint");
-        joint_msg->velocities.push_back(joint_vel_cmd_);
-        publish_joint = true;
-        break;
-      case KEYCODE_4:
-        RCLCPP_DEBUG(nh_->get_logger(), "4");
-        joint_msg->joint_names.push_back("wrist_1_joint");
-        joint_msg->velocities.push_back(joint_vel_cmd_);
-        publish_joint = true;
-        break;
-      case KEYCODE_5:
-        RCLCPP_DEBUG(nh_->get_logger(), "5");
-        joint_msg->joint_names.push_back("wrist_2_joint");
-        joint_msg->velocities.push_back(joint_vel_cmd_);
-        publish_joint = true;
-        break;
-      case KEYCODE_6:
-        RCLCPP_DEBUG(nh_->get_logger(), "6");
-        joint_msg->joint_names.push_back("wrist_3_joint");
-        joint_msg->velocities.push_back(joint_vel_cmd_);
-        publish_joint = true;
-        break;
-//      case KEYCODE_7:
-//        RCLCPP_DEBUG(nh_->get_logger(), "7");
-//        joint_msg->joint_names.push_back("finger_joint");
-//        joint_msg->velocities.push_back(joint_vel_cmd_);
-//        publish_joint = true;
-//        break;
-      case KEYCODE_R:
-        RCLCPP_DEBUG(nh_->get_logger(), "R");
-        joint_vel_cmd_ *= -1;
-        break;
-      case KEYCODE_Q:
-        RCLCPP_DEBUG(nh_->get_logger(), "quit");
-        return 0;
-      // Add cases for other keys as needed
-      case KEYCODE_PLUS:
-        RCLCPP_DEBUG(nh_->get_logger(), "PLUS");
-        handlePlusPress();
-        break;
-      case KEYCODE_MINUS:
-        RCLCPP_DEBUG(nh_->get_logger(), "MINUS");
-        handleMinusPress();
-        break;
-    }
-
-    // If a key requiring a publish was pressed, publish the message now
-    if (publish_twist)
-    {
-      twist_msg->header.stamp = nh_->now();
-      twist_msg->header.frame_id = frame_to_publish_;
-      twist_pub_->publish(std::move(twist_msg));
-      publish_twist = false;
-    }
-    else if (publish_joint)
-    {
-      joint_msg->header.stamp = nh_->now();
-      joint_msg->header.frame_id = BASE_FRAME_ID;
-      joint_pub_->publish(std::move(joint_msg));
-      publish_joint = false;
-    }
-  }
-
-  return 0;
-}

src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml

@@ -1,233 +0,0 @@
-controller_manager:
-  ros__parameters:
-    force_torque_sensor_broadcaster:
-      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
-
-    scaled_joint_trajectory_controller:
-      type: ur_controllers/ScaledJointTrajectoryController
-
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
-
-    cartesian_compliance_controller:
-      type: cartesian_compliance_controller/CartesianComplianceController
-
-    cartesian_force_controller:
-      type: cartesian_force_controller/CartesianForceController
-
-    cartesian_motion_controller:
-      type: cartesian_motion_controller/CartesianMotionController
-
-    motion_control_handle:
-      type: cartesian_controller_handles/MotionControlHandle
-
-    joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-
-
-force_torque_sensor_broadcaster:
-  ros__parameters:
-    sensor_name: tcp_fts_sensor
-    state_interface_names:
-      - force.x
-      - force.y
-      - force.z
-      - torque.x
-      - torque.y
-      - torque.z
-    frame_id: tool0
-    topic_name: wrench
-
-scaled_joint_trajectory_controller:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity
-    state_publish_rate: 100.0
-    action_monitor_rate: 20.0
-    allow_partial_joints_goal: false
-    constraints:
-      stopped_velocity_tolerance: 0.2
-      goal_time: 0.0
-      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
-      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
-      elbow_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
-
-cartesian_compliance_controller:
-  ros__parameters:
-
-    # This is the tip of the robot tool that you usually use for your task.
-    # For instance, it could be the drilling bit of a screwdriver or a grinding
-    # tool.  When you specify a target_wrench, i.e. some additional forces that
-    # your robot should apply to its environment, that target_wrench gets
-    # applied in this frame.
-    end_effector_link: "tool0"
-
-    # This is usually the link directly before the first actuated joint.  All
-    # controllers will build a kinematic chain from this link up to
-    # end_effector_link.  It's also the reference frame for the superposition
-    # of error components in all controllers.
-    robot_base_link: "base_link"
-
-    # This is the URDF link of your sensor. Sensor signals are assumed to be
-    # given in this frame.  It's important that this link is located somewhere
-    # between end_effector_link and robot_base_link.  If that's not the case,
-    # the controllers won't initialize and will emit an error message.
-    ft_sensor_ref_link: "tool0"
-
-    # This is the link that the robot feels compliant about. It does not need
-    # to coincide with the end_effector_link, but for many use cases, this
-    # configuration is handy. When working with a screwdriver, for instance,
-    # setting compliance_ref_link == end_effector_link makes it easy to specify
-    # downward pushing forces without generating unwanted offset moments.
-    # On the other hand, an application could benefit from yielding a little in
-    # the robot's wrist while drawing a line on a surface with a pen.
-    compliance_ref_link: "tool0"
-
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    # Choose between position or velocity.  In fact, the controllers allow to
-    # set both at the same time, but not all drivers will support this.
-    # In general, position control is a little smoother due to the double
-    # time-integrated commands from the solver. If available on a robot, it
-    # should be the default.  On some drivers, the velocity interface provides
-    # faster control cycles, and hence could improve stability in
-    # contact-dominated tasks.  A drawback is that we lose one time integration
-    # step here and obtain noisier command signals in comparison to the
-    # position interface.  It's probably suitable to test both on a new robot
-    # and decide for what works best.
-    command_interfaces:
-      - position
-        #- velocity
-
-    stiffness:  # w.r.t. compliance_ref_link coordinates
-        trans_x: 500.0
-        trans_y: 500.0
-        trans_z: 500.0
-        rot_x: 20.0
-        rot_y: 20.0
-        rot_z: 20.0
-
-    solver:
-        error_scale: 0.5
-        iterations: 1
-        publish_state_feedback: True
-
-    # For all controllers, these gains are w.r.t. the robot_base_link coordinates.
-    pd_gains:
-        trans_x: {p: 0.05, d: 0.005}
-        trans_y: {p: 0.05, d: 0.005}
-        trans_z: {p: 0.05, d: 0.005}
-        rot_x: {p: 1.5}
-        rot_y: {p: 1.5}
-        rot_z: {p: 1.5}
-
-cartesian_force_controller:
-  ros__parameters:
-
-    # See the cartesian_compliance_controller
-    end_effector_link: "tool0"
-    robot_base_link: "base_link"
-    ft_sensor_ref_link: "tool0"
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    # See the cartesian_compliance_controller
-    command_interfaces:
-      - position
-        #- velocity
-
-    solver:
-        error_scale: 0.5
-        publish_state_feedback: True
-
-    pd_gains:
-        trans_x: {p: 0.05}
-        trans_y: {p: 0.05}
-        trans_z: {p: 0.05}
-        rot_x: {p: 1.5}
-        rot_y: {p: 1.5}
-        rot_z: {p: 1.5}
-
-cartesian_motion_controller:
-  ros__parameters:
-
-    # See the cartesian_compliance_controller
-    end_effector_link: "tool0"
-    robot_base_link: "base_link"
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    # See the cartesian_compliance_controller
-    command_interfaces:
-      - position
-        #- velocity
-
-    solver:
-        error_scale: 1.0
-        iterations: 10
-        publish_state_feedback: True
-
-    pd_gains:
-        trans_x: {p: 1.0}
-        trans_y: {p: 1.0}
-        trans_z: {p: 1.0}
-        rot_x: {p: 0.5}
-        rot_y: {p: 0.5}
-        rot_z: {p: 0.5}
-
-motion_control_handle:
-  ros__parameters:
-    end_effector_link: "tool0"
-    robot_base_link: "base_link"
-    ft_sensor_ref_link: "tool0"
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-joint_trajectory_controller:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity

src/ur_robotiq_description/config/ur_robotiq_controllers.yaml

@@ -25,16 +25,13 @@ controller_manager:
       type: position_controllers/JointGroupPositionController
 
     forward_gripper_position_controller:
-      type: position_controllers/JointGroupPositionController
+      type: robotiq_2f_controllers/ForwardController
 
     robotiq_gripper_controller:
-      type: position_controllers/GripperActionController
+      type: robotiq_2f_controllers/GripperCommand
-
-    robotiq_gripper_joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
 
     robotiq_activation_controller:
-      type: robotiq_controllers/RobotiqActivationController
+      type: robotiq_2f_controllers/RobotiqActivationController
 
 speed_scaling_state_broadcaster:
   ros__parameters:
@@ -138,32 +135,12 @@ forward_position_controller:
 
 forward_gripper_position_controller:
   ros__parameters:
-    joints:
+    joint: $(var prefix)gripper_gap
-      - $(var tf_prefix)finger_joint
-
 
 robotiq_gripper_controller:
   ros__parameters:
-    joint: $(var tf_prefix)finger_joint
+    joint: $(var prefix)gripper_gap
-    use_effort_interface: true
+    max_gap: 0.14
-    use_speed_interface: true
-
-robotiq_gripper_joint_trajectory_controller:
-  ros__parameters:
-    joints:
-     - $(var tf_prefix)finger_joint
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity
-    state_publish_rate: 100.0
-    action_monitor_rate: 20.0
-    allow_partial_joints_goal: false
-    constraints:
-      stopped_velocity_tolerance: 0.2
-      goal_time: 0.0
-      $(var tf_prefix)finger_joint: { trajectory: 0.2, goal: 0.1 }
 
 robotiq_activation_controller:
   ros__parameters:

src/ur_robotiq_description/launch/ur_robotiq_control.launch.py

@@ -6,8 +6,7 @@ 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
-# from moveit_configs_utils import MoveItConfigsBuilder
+
-# from launch_param_builder import ParameterBuilder
 
 def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
@@ -166,12 +165,14 @@ def launch_setup(context, *args, **kwargs):
             "trajectory_port:=",
             trajectory_port,
             " ",
+            "prefix:=",
+            tf_prefix,
+            " ",
         ]
     )
 
     robot_description = {"robot_description": robot_description_content}
 
-
     initial_joint_controllers = PathJoinSubstitution(
         [FindPackageShare(description_package), "config", controllers_file]
     )
@@ -216,13 +217,6 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="spawner",
         arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
-        condition=UnlessCondition(use_fake_hardware),
-    )
-    robotiq_gripper_joint_trajectory_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["robotiq_gripper_joint_trajectory_controller", "-c", "/controller_manager"],
-        condition=IfCondition(use_fake_hardware),
     )
 
     robotiq_activation_controller_spawner = Node(
@@ -242,20 +236,6 @@ def launch_setup(context, *args, **kwargs):
         parameters=[{"robot_ip": robot_ip}],
     )
 
-    tool_communication_node = Node(
-        package="ur_robot_driver",
-        condition=IfCondition(use_tool_communication),
-        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,
-            }
-        ],
-    )
 
     controller_stopper_node = Node(
         package="ur_robot_driver",
@@ -277,38 +257,6 @@ def launch_setup(context, *args, **kwargs):
             },
         ],
     )
-    ############################################################################################################
-    # # Get parameters for the Servo node
-    # servo_params = (
-    #     ParameterBuilder("ur_robotiq_servo")
-    #     .yaml(
-    #         parameter_namespace="moveit_servo",
-    #         file_path="config/ur_robotiq_simulated_config.yaml",
-    #     )
-    #     .to_dict()
-    # )
-    # print(servo_params)
-    #
-    # # A node to publish world -> panda_link0 transform
-    # static_tf = Node(
-    #     package="tf2_ros",
-    #     executable="static_transform_publisher",
-    #     name="static_transform_publisher",
-    #     output="log",
-    #     arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
-    # )
-    #
-    # # The servo cpp interface demo
-    # # Creates the Servo node and publishes commands to it
-    # servo_node = Node(
-    #     package="moveit2_tutorials",
-    #     executable="servo_cpp_interface_demo",
-    #     output="screen",
-    #     parameters=[
-    #         servo_params,
-    #         robot_description,
-    #     ],
-    # )
 
     robot_state_publisher_node = Node(
         package="robot_state_publisher",
@@ -387,14 +335,11 @@ def launch_setup(context, *args, **kwargs):
                          ur_control_node,
                          dashboard_client_node,
                          controller_stopper_node,
-                            # static_tf,
-                            # servo_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
 
@@ -487,6 +432,15 @@ def generate_launch_description():
         have to be updated.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value="",
+            description="prefix of the joint names, useful for \
+        multi-robot setup. If changed, also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_fake_hardware",

src/ur_robotiq_description/package.xml

@@ -23,9 +23,9 @@
   <exec_depend>ur_description</exec_depend>
   <exec_depend>ur_robot_driver</exec_depend>
   <exec_depend>ur_controllers</exec_depend>
-  <exec_depend>robotiq_description</exec_depend>
+  <exec_depend>robotiq_2f_description</exec_depend>
-  <exec_depend>robotiq_driver</exec_depend>
+  <exec_depend>robotiq_2f_interface</exec_depend>
-  <exec_depend>robotiq_controllers</exec_depend>
+  <exec_depend>robotiq_2f_controllers</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
 

src/ur_robotiq_description/urdf/ur_robotiq.urdf

@@ -1,9 +1,9 @@
 <?xml version="1.0" ?>
 <!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro | -->
+<!-- |    This document was autogenerated by xacro from ur_robotiq.urdf.xacro          | -->
 <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
 <!-- =================================================================================== -->
-<robot name="ur">
+<robot name="ur_manipulator">
   <!--
     Base UR robot series xacro macro.
 
@@ -59,7 +59,7 @@
           this location. Nor can they rely on the existence of the macro.
   -->
   <link name="world"/>
-  <ros2_control name="ur" type="system">
+  <ros2_control name="ur_manipulator" type="system">
     <hardware>
       <plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
       <param name="robot_ip">0.0.0.0</param>
@@ -99,7 +99,7 @@
       </command_interface>
       <state_interface name="position">
         <!-- initial position for the FakeSystem and simulation -->
-        <param name="initial_value">1.4</param>
+        <param name="initial_value">0.0</param>
       </state_interface>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>
@@ -131,7 +131,7 @@
       </command_interface>
       <state_interface name="position">
         <!-- initial position for the FakeSystem and simulation -->
-        <param name="initial_value">1.57</param>
+        <param name="initial_value">0.0</param>
       </state_interface>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>
@@ -163,7 +163,7 @@
       </command_interface>
       <state_interface name="position">
         <!-- initial position for the FakeSystem and simulation -->
-        <param name="initial_value">-1.57</param>
+        <param name="initial_value">0.0</param>
       </state_interface>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>
@@ -591,7 +591,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_base_link.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_base_link.stl"/>
       </geometry>
       <material name="">
         <color rgba="0.1 0.1 0.1 1"/>
@@ -600,7 +600,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_base_link.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_base_link.stl"/>
       </geometry>
     </collision>
   </link>
@@ -618,7 +618,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
       </geometry>
       <material name="">
         <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@@ -627,7 +627,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
       </geometry>
     </collision>
   </link>
@@ -640,7 +640,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
       </geometry>
       <material name="">
         <color rgba="0.1 0.1 0.1 1"/>
@@ -649,7 +649,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -662,7 +662,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
       </geometry>
       <material name="">
         <color rgba="0.1 0.1 0.1 1"/>
@@ -671,7 +671,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -704,7 +704,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
       </geometry>
       <material name="">
         <color rgba="0.1 0.1 0.1 1"/>
@@ -713,7 +713,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
       </geometry>
     </collision>
   </link>
@@ -726,7 +726,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
       </geometry>
       <material name="">
         <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@@ -735,7 +735,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl"/>
       </geometry>
     </collision>
   </link>
@@ -748,7 +748,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl"/>
       </geometry>
       <material name="">
         <color rgba="0.1 0.1 0.1 1"/>
@@ -757,7 +757,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -770,7 +770,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl"/>
       </geometry>
       <material name="">
         <color rgba="0.1 0.1 0.1 1"/>
@@ -779,7 +779,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl"/>
       </geometry>
     </collision>
   </link>
@@ -812,7 +812,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
       </geometry>
       <material name="">
         <color rgba="0.1 0.1 0.1 1"/>
@@ -821,7 +821,7 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="file:///home/fotis/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
+        <mesh filename="file:///home/niko/PycharmProjects/UR_Robotiq/install/robotiq_description/share/robotiq_description/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl"/>
       </geometry>
     </collision>
   </link>

src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro

@@ -1,11 +1,11 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
 
-	<!-- parameters -->    
+	<!-- parameters -->
 	<xacro:arg name="name" default="ur_manipulator"/>
 	<!-- import main macro -->
 	<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
-	<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
+	<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
 
 	<!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20 -->
 	<!-- the default value should raise an error in case this was called without defining the type -->
@@ -33,15 +33,9 @@
 	<xacro:arg name="script_sender_port" default="50002"/>
 	<xacro:arg name="trajectory_port" default="50003"/>
 	<!--   tool communication related parameters-->
-	<xacro:arg name="use_tool_communication" default="false" />
+	<xacro:arg name="use_tool_communication" default="true" />
-	<xacro:arg name="tool_voltage" default="0" />
+	<xacro:arg name="tool_voltage" default="24" />
-	<xacro:arg name="tool_parity" default="0" />
+	<xacro:arg name="tool_tcp_port" default="63352" />
-	<xacro:arg name="tool_baud_rate" default="115200" />
-	<xacro:arg name="tool_stop_bits" default="1" />
-	<xacro:arg name="tool_rx_idle_chars" default="1.5" />
-	<xacro:arg name="tool_tx_idle_chars" default="3.5" />
-	<xacro:arg name="tool_device_name" default="/tmp/ttyUR" />
-	<xacro:arg name="tool_tcp_port" default="54321" />
 
 	<!-- Simulation parameters -->
 	<xacro:arg name="use_fake_hardware" default="false" />
@@ -78,12 +72,6 @@
      initial_positions="${xacro.load_yaml(initial_positions_file)}"
      use_tool_communication="$(arg use_tool_communication)"
      tool_voltage="$(arg tool_voltage)"
-     tool_parity="$(arg tool_parity)"
-     tool_baud_rate="$(arg tool_baud_rate)"
-     tool_stop_bits="$(arg tool_stop_bits)"
-     tool_rx_idle_chars="$(arg tool_rx_idle_chars)"
-     tool_tx_idle_chars="$(arg tool_tx_idle_chars)"
-     tool_device_name="$(arg tool_device_name)"
      tool_tcp_port="$(arg tool_tcp_port)"
      robot_ip="$(arg robot_ip)"
      script_filename="$(arg script_filename)"
@@ -102,6 +90,8 @@
     prefix="$(arg tf_prefix)"
     parent="$(arg tf_prefix)tool0"
     use_fake_hardware="$(arg use_fake_hardware)"
+    robot_ip="$(arg robot_ip)"
+    robot_port="$(arg tool_tcp_port)"
     >
     <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:robotiq_gripper>

src/ur_robotiq_moveit_config/bash/servo_startup.sh

@@ -0,0 +1,51 @@
+#!/bin/bash
+
+# Create a Tilix layout file in JSON format
+layout_file=$(mktemp)
+cat <<EOL > $layout_file
+{
+  "type": "Window",
+  "tabs": [
+    {
+      "type": "Terminal",
+      "children": [
+        {
+          "type": "Terminal",
+          "children": [
+            {
+              "type": "Terminal",
+              "command": "bash -c 'cd ~/PycharmProjects/UR_Robotiq && . install/setup.bash; exec bash'"
+            },
+            {
+              "type": "Terminal",
+              "command": "bash -c 'cd ~/PycharmProjects/UR_Robotiq && . install/setup.bash; exec bash'"
+            }
+          ],
+          "orientation": "vertical"
+        },
+        {
+          "type": "Terminal",
+          "children": [
+            {
+              "type": "Terminal",
+              "command": "bash -c 'cd ~/PycharmProjects/UR_Robotiq && . install/setup.bash; exec bash'"
+            },
+            {
+              "type": "Terminal",
+              "command": "bash -c 'cd ~/PycharmProjects/UR_Robotiq && . install/setup.bash; exec bash'"
+            }
+          ],
+          "orientation": "vertical"
+        }
+      ],
+      "orientation": "horizontal"
+    }
+  ]
+}
+EOL
+
+# Open Tilix with the specified layout
+tilix --session $layout_file
+
+# Remove the temporary layout file
+rm $layout_file

src/ur_robotiq_moveit_config/config/initial_positions.yaml

@@ -4,4 +4,4 @@ elbow_joint: 1.57
 wrist_1_joint: -1.57
 wrist_2_joint: -1.57
 wrist_3_joint: 0.0
-finger_joint: 20.0
+gripper_gap: 0.0

src/ur_robotiq_moveit_config/config/joint_limits.yaml

@@ -1,50 +1,90 @@
-# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+# Joints limits
-
+#
-# For beginners, we downscale velocity and acceleration limits.
+# Sources:
-# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+#
-default_velocity_scaling_factor: 0.1
+#  - Universal Robots e-Series, User Manual, UR3e, Version 5.8
-default_acceleration_scaling_factor: 0.1
+#    https://s3-eu-west-1.amazonaws.com/ur-support-site/69043/99403_UR3e_User_Manual_en_Global.pdf
-
+#  - Support > Articles > UR articles > Max. joint torques
-# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+#    https://www.universal-robots.com/articles/ur-articles/max-joint-torques
-# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+#    retrieved: 2020-06-16, last modified: 2020-06-09
 joint_limits:
-  elbow_joint:
-    has_velocity_limits: true
-    max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
-  left_inner_knuckle_joint:
-    has_velocity_limits: true
-    max_velocity: 2
-    has_acceleration_limits: false
-    max_acceleration: 0
-  right_inner_knuckle_joint:
-    has_velocity_limits: true
-    max_velocity: 2
-    has_acceleration_limits: false
-    max_acceleration: 0
-  shoulder_lift_joint:
-    has_velocity_limits: true
-    max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
   shoulder_pan_joint:
-    has_velocity_limits: true
+    # acceleration limits are not publicly available
-    max_velocity: 3.1415926535897931
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 56.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  180.0
+    min_position: !degrees -360.0
+  shoulder_lift_joint:
+    # acceleration limits are not publicly available
+    has_acceleration_limits: false
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 56.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  180.0
+    min_position: !degrees -360.0
+  elbow_joint:
+    # acceleration limits are not publicly available
+    has_acceleration_limits: false
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 28.0
+    # we artificially limit this joint to half its actual joint position limit
+    # to avoid (MoveIt/OMPL) planning problems, as due to the physical
+    # construction of the robot, it's impossible to rotate the 'elbow_joint'
+    # over more than approx +- 1 pi (the shoulder lift joint gets in the way).
+    #
+    # This leads to planning problems as the search space will be divided into
+    # two sections, with no connections from one to the other.
+    #
+    # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for
+    # more information.
+    max_position: !degrees  180.0
+    max_velocity: !degrees  180.0
+    min_position: !degrees -180.0
   wrist_1_joint:
-    has_velocity_limits: true
+    # acceleration limits are not publicly available
-    max_velocity: 6.2831853071795862
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 12.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  360.0
+    min_position: !degrees -360.0
   wrist_2_joint:
-    has_velocity_limits: true
+    # acceleration limits are not publicly available
-    max_velocity: 6.2831853071795862
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 12.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  360.0
+    min_position: !degrees -360.0
   wrist_3_joint:
-    has_velocity_limits: true
+    # acceleration limits are not publicly available
-    max_velocity: 6.2831853071795862
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 12.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  360.0
+    min_position: !degrees -360.0
+  gripper_gap:
+    # acceleration limits are not publicly available
+    has_acceleration_limits: false
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 235.0
+    max_position: 0.14
+    max_velocity: 0.15
+    min_position: 0.0

src/ur_robotiq_moveit_config/config/moveit_controllers.yaml

@@ -1,10 +1,7 @@
-
 controller_names:
   - scaled_joint_trajectory_controller
   - joint_trajectory_controller
   - robotiq_gripper_controller
-  - robotiq_gripper_joint_trajectory_controller
-  - combined_joint_trajectory_controller
 
 
 scaled_joint_trajectory_controller:
@@ -32,16 +29,10 @@ joint_trajectory_controller:
     - wrist_2_joint
     - wrist_3_joint
 
+
 robotiq_gripper_controller:
   action_ns: gripper_cmd
   type: GripperCommand
   default: true
   joints:
-    - finger_joint
+    - gripper_gap
-
-robotiq_gripper_joint_trajectory_controller:
-  action_ns: follow_joint_trajectory
-  type: FollowJointTrajectory
-  default: false
-  joints:
-    - finger_joint

src/ur_robotiq_moveit_config/config/ur_servo.yaml

@@ -2,15 +2,15 @@
 # Modify all parameters related to servoing here
 ###############################################
 use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
-use_intra_process_comms : true
+use_intra_process_comms : True
 ## Properties of incoming commands
 command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
-  linear:  0.00001  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  linear:  0.4  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
-  rotational:  0.001 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  rotational:  3.2 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
-  joint: 0.01
+  joint: 0.5
 # This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
 # controller performance. It essentially increases the timestep when calculating the target pose, to move the target
 # pose farther away. [seconds]
@@ -38,6 +38,7 @@ low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the
 ## MoveIt properties
 move_group_name:  ur_manipulator  # Often 'manipulator' or 'arm'
 planning_frame: world  # The MoveIt planning frame. Often 'base_link' or 'world'
+is_primary_planning_scene_monitor: false
 
 ## Other frames
 ee_frame_name: tool0  # The name of the end effector link, used to return the EE pose

src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py

@@ -40,17 +40,17 @@ def launch_setup(context, *args, **kwargs):
     launch_servo = LaunchConfiguration("launch_servo")
 
     # File and package names
-    ur_description_package = "ur_description"
+    ur_description_package = LaunchConfiguration("ur_description_package")
-    ur_robotiq_description_package = "ur_robotiq_description"
+    ur_robotiq_description_package = LaunchConfiguration("ur_robotiq_description_package")
-    ur_robotiq_description_file = "ur_robotiq.urdf.xacro"
+    ur_robotiq_description_file = LaunchConfiguration("ur_robotiq_description_file")
-    moveit_config_package = "ur_robotiq_moveit_config"
+    moveit_config_package = LaunchConfiguration("moveit_config_package")
-    moveit_config_file = "ur_robotiq.srdf.xacro"
+    moveit_config_file = LaunchConfiguration("moveit_config_file")
 
     joint_limit_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
     )
     kinematics_params = PathJoinSubstitution(
-        [FindPackageShare(ur_description_package), "config", ur_type, "default_kinematics.yaml"]
+        [FindPackageShare(ur_robotiq_description_package), "config", "ur_robotiq_calibration.yaml"]
     )
     physical_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"]
@@ -59,6 +59,8 @@ def launch_setup(context, *args, **kwargs):
         [FindPackageShare(ur_description_package), "config", ur_type, "visual_parameters.yaml"]
     )
 
+
+
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -126,7 +128,7 @@ def launch_setup(context, *args, **kwargs):
 
     robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
 
-    robot_description_kinematics_params = load_yaml(moveit_config_package, "config/kinematics.yaml")
+    robot_description_kinematics_params = load_yaml("ur_robotiq_moveit_config", "config/kinematics.yaml")
     robot_description_kinematics = {"robot_description_kinematics": robot_description_kinematics_params}
     # robot_description_planning = {
     #     "robot_description_planning": load_yaml(ur_moveit_config_package, "config/joint_limits.yaml")
@@ -140,20 +142,17 @@ def launch_setup(context, *args, **kwargs):
             "start_state_max_bounds_error": 0.1,
         }
     }
-
+    ompl_planning_yaml = load_yaml("ur_robotiq_moveit_config", "config/ompl_planning.yaml")
-    ompl_planning_yaml = load_yaml(moveit_config_package, "config/ompl_planning.yaml")
     ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
 
     # Trajectory Execution Configuration
     # the scaled_joint_trajectory_controller does not work on fake hardware
     use_fake_hw = use_fake_hardware.perform(context)
-    controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml")
+    controllers_yaml = load_yaml("ur_robotiq_moveit_config", "config/moveit_controllers.yaml")
 
-    if use_fake_hw == "true":
+    if use_fake_hw == "true" or use_fake_hw == "True":
         controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
         controllers_yaml["joint_trajectory_controller"]["default"] = True
-        controllers_yaml["robotiq_gripper_controller"]["default"] = False
-        controllers_yaml["robotiq_gripper_joint_trajectory_controller"]["default"] = True
 
     moveit_controllers = {
         "moveit_simple_controller_manager": controllers_yaml,
@@ -221,7 +220,7 @@ def launch_setup(context, *args, **kwargs):
 
     # Servo node for realtime control
 
-    servo_yaml = load_yaml(moveit_config_package, "config/ur_servo.yaml")
+    servo_yaml = load_yaml("ur_robotiq_moveit_config", "config/ur_servo.yaml")
     servo_params = {"moveit_servo": servo_yaml}
     servo_node = Node(
         package="moveit_servo",
@@ -243,7 +242,46 @@ def launch_setup(context, *args, **kwargs):
 def generate_launch_description():
     # Declare the Launch arguments
     declared_arguments = []
-    # UR specific arguments
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_description_package",
+            description="Description package with robot URDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom description.",
+            default_value="ur_description",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_robotiq_description_package",
+            description="Description package with robot and gripper URDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom description.",
+            default_value="ur_robotiq_description",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_robotiq_description_file",
+            description="URDF/XACRO description file with the robot.",
+            default_value="ur_robotiq.urdf.xacro",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_package",
+            description="MoveIt config package with robot and gripper SRDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom moveit config.",
+            default_value="ur_robotiq_moveit_config",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_file",
+            description="MoveIt SRDF/XACRO description file with the robot and gripper.",
+            default_value="ur_robotiq.srdf.xacro",
+        )
+    )
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "ur_type",

src/ur_robotiq_moveit_config/package.xml

@@ -45,9 +45,9 @@
   <exec_depend>ur_description</exec_depend>
   <exec_depend>ur_robot_driver</exec_depend>
   <exec_depend>ur_controllers</exec_depend>
-  <exec_depend>robotiq_description</exec_depend>
+  <exec_depend>robotiq_2f_description</exec_depend>
-  <exec_depend>robotiq_driver</exec_depend>
+  <exec_depend>robotiq_2f_interface</exec_depend>
-  <exec_depend>robotiq_controllers</exec_depend>
+  <exec_depend>robotiq_2f_controllers</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
 
   <depend>control_msgs</depend>

src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro

@@ -1,134 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
-  <xacro:macro name="ur_robotiq_srdf" params="name prefix">
-    <!--GROUPS - Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS - When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS - When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS - When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS - Groups can also be formed by referencing to already defined group names-->
-    <group name="${prefix}${name}_manipulator">
-      <chain base_link="${prefix}base_link" tip_link="${prefix}tool0" />
-    </group>
-    <group name="${prefix}gripper">
-        <joint name="${prefix}finger_joint"/>
-        <joint name="${prefix}left_outer_finger_joint" />
-        <joint name="${prefix}left_inner_knuckle_joint" />
-        <joint name="${prefix}left_inner_finger_joint" />
-        <joint name="${prefix}right_outer_knuckle_joint" />
-        <joint name="${prefix}right_outer_finger_joint" />
-        <joint name="${prefix}right_inner_knuckle_joint" />
-        <joint name="${prefix}right_inner_finger_joint" />
-    </group>
-    <!--GROUP STATES - Purpose - Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="${prefix}home" group="${prefix}${name}_manipulator">
-      <joint name="${prefix}elbow_joint" value="0" />
-      <joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
-      <joint name="${prefix}shoulder_pan_joint" value="0" />
-      <joint name="${prefix}wrist_1_joint" value="0" />
-      <joint name="${prefix}wrist_2_joint" value="0" />
-      <joint name="${prefix}wrist_3_joint" value="0" />
-    </group_state>
-    <group_state name="${prefix}up" group="${prefix}${name}_manipulator">
-      <joint name="${prefix}elbow_joint" value="0" />
-      <joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
-      <joint name="${prefix}shoulder_pan_joint" value="0" />
-      <joint name="${prefix}wrist_1_joint" value="-1.5707" />
-      <joint name="${prefix}wrist_2_joint" value="0" />
-      <joint name="${prefix}wrist_3_joint" value="0" />
-    </group_state>
-    <group_state name="${prefix}test_configuration" group="${prefix}${name}_manipulator">
-      <joint name="${prefix}elbow_joint" value="1.4" />
-      <joint name="${prefix}shoulder_lift_joint" value="-1.62" />
-      <joint name="${prefix}shoulder_pan_joint" value="1.54" />
-      <joint name="${prefix}wrist_1_joint" value="-1.2" />
-      <joint name="${prefix}wrist_2_joint" value="-1.6" />
-      <joint name="${prefix}wrist_3_joint" value="-0.11" />
-    </group_state>
-        <group_state name="open" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0" />
-    </group_state>
-    <group_state name="closed" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0.698132" />
-    </group_state>
-    <!--END EFFECTOR - Purpose - Represent information about an end effector.-->
-    <end_effector name="${prefix}gripper" parent_link="${prefix}tool0" group="${prefix}gripper" parent_group="${prefix}${name}_manipulator" />
-    <!--VIRTUAL JOINT - Purpose - this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link" />
-    <!--PASSIVE JOINT - Purpose - this element is used to mark joints that are not actuated-->
-    <passive_joint name="${prefix}left_outer_finger_joint" />
-    <passive_joint name="${prefix}left_inner_knuckle_joint" />
-    <passive_joint name="${prefix}left_inner_finger_joint" />
-    <passive_joint name="${prefix}right_outer_knuckle_joint" />
-    <passive_joint name="${prefix}right_outer_finger_joint" />
-    <passive_joint name="${prefix}right_inner_knuckle_joint" />
-    <passive_joint name="${prefix}right_inner_finger_joint" />
-    <!--DISABLE COLLISIONS - By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <!--UR-Robot-->
-    <disable_collisions link1="${prefix}base" link2="${prefix}base_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link" link2="${prefix}base_link_inertia" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}shoulder_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_1_link" reason="Never" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_2_link" reason="Never" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_3_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}wrist_1_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}shoulder_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_2_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never" />
-    <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
-    <!--Robotiq-Gripper-->
-    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="left_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="right_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
-
-    <!-- Weird Bug-->
-    <disable_collisions link1="${prefix}base" link2="${prefix}base" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link" link2="${prefix}base_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}base_link_inertia" reason="Adjacent" />
-    <disable_collisions link1="${prefix}shoulder_link" link2="${prefix}shoulder_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}upper_arm_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}forearm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_1_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_2_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}flange" link2="${prefix}flange" reason="Adjacent" />
-    <disable_collisions link1="${prefix}ft_frame" link2="${prefix}ft_frame" reason="Adjacent" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}tool0" reason="Adjacent" />
-
-    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="robotiq_140_base_link" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
-  </xacro:macro>
-</robot>