build complete

now launch debugging

.idea/vcs.xml

@@ -5,7 +5,5 @@
     <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>

src/robotiq_2f/robotiq_2f_description/CMakeLists.txt

@@ -5,22 +5,16 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
 
+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

@@ -1,5 +1,4 @@
-robotiq_2f_140_gripper:
+robotiq_2f_gripper:
-  ros__parameters:
   # Physical limits
   min_position: 0.0     # Meters (fully closed)
   max_position: 0.14    # Meters (fully open)

src/robotiq_2f/robotiq_2f_description/package.xml

@@ -9,6 +9,20 @@
 
   <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>ros2_control</exec_depend>
+  <exec_depend>ros2_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>
 

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

@@ -13,7 +13,7 @@
             <!-- Plugins -->
             <hardware>
                 <xacro:if value="${use_fake_hardware}">
-                    <plugin>mock_components/GenericSystem</plugin>
+                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
                     <param name="fake_sensor_commands">${fake_sensor_commands}</param>
                     <param name="state_following_offset">0.0</param>
                 </xacro:if>

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

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
+    <xacro:include filename="$(find robotiq_2f_description)/config/robotiq_2f_140_config.yaml" />
     <xacro:macro name="robotiq_gripper" params="
         name
         prefix
@@ -20,7 +21,13 @@
                 use_fake_hardware="${use_fake_hardware}"
                 fake_sensor_commands="${fake_sensor_commands}"
                 robot_ip="${robot_ip}"
-                robot_port="${robot_port}"/>
+                robot_port="${robot_port}"
+                min_position="${robotiq_2f_gripper.min_position}"
+                max_position="${robotiq_2f_gripper.max_position}"
+                min_speed="${robotiq_2f_gripper.min_speed}"
+                max_speed="${robotiq_2f_gripper.max_speed}"
+                min_force="${robotiq_2f_gripper.min_force}"
+                max_force="${robotiq_2f_gripper.max_force}"/>
         </xacro:if>
 
         <!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->

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

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
+    <xacro:include filename="$(find robotiq_2f_description)/config/robotiq_2f_85_config.yaml" />
     <xacro:macro name="robotiq_gripper" params="
         name
         prefix
@@ -20,7 +21,13 @@
                 use_fake_hardware="${use_fake_hardware}"
                 fake_sensor_commands="${fake_sensor_commands}"
                 robot_ip="${robot_ip}"/>
-                robot_port="${robot_port}"/>
+                robot_port="${robot_port}"
+                min_position="${robotiq_2f_gripper.min_position}"
+                max_position="${robotiq_2f_gripper.max_position}"
+                min_speed="${robotiq_2f_gripper.min_speed}"
+                max_speed="${robotiq_2f_gripper.max_speed}"
+                min_force="${robotiq_2f_gripper.min_force}"
+                max_force="${robotiq_2f_gripper.max_force}"/>
         </xacro:if>
 
         <link name="${prefix}robotiq_85_base_link">

src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml

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

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

@@ -30,15 +30,6 @@ public:
         isConnected.store(false);
     }
 
-    int getGripperPosition() override {
-        return gripperPosition.load();
-    }
-
-    int getGripperForce() override {
-        // Assuming this is supposed to mimic `force()`
-        return gripperForce.load();
-    }
-
     int force() override {
         return gripperForce.load();
     }

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

@@ -237,7 +237,7 @@ private:
 
     // Smart pointer to manage the socket descriptor with the custom deleter
     std::unique_ptr<int, SocketDeleter> sockfd_;
-    std::mutex socketMutex_; // Mutex for socket access synchronization
+    std::mutex socket_mutex_; // Mutex for socket access synchronization
 
     // bounds of the encoded gripper states
     int min_position_ = 0;
@@ -351,6 +351,8 @@ private:
 
     // Constants buffer sizes
     const size_t BUFFER_SIZE = 1024; // Adjust as necessary
+    const int MAX_RETRIES = 5;
+    const int RETRY_DELAY_MS = 20;
 };
 } // end namespace
 

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

@@ -10,13 +10,17 @@
 #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/actuator_interface.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 <stdexcept>
@@ -48,11 +52,12 @@ protected: // Likely changes the access to protected from private
     // Members for driver interaction
     bool use_mock_hardware_;
     std::unique_ptr<SocketAdapterBase> socket_adapter_;
-    void RobotiqGripperHardwareInterface::configureAdapter(bool useMock);
+    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();
 
     static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
@@ -97,10 +102,10 @@ protected: // Likely changes the access to protected from private
     double max_force_;
 
     // loop time
-    constexpr auto gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
+    const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
 
     // Logger
-    const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
+    const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
 };
 } // end namespace
 

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_DRIVER__VISIBILITY_CONTROL_HPP_
+#define ROBOTIQ_DRIVER__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_DRIVER_EXPORT __attribute__((dllexport))
+#define ROBOTIQ_DRIVER_IMPORT __attribute__((dllimport))
+#else
+#define ROBOTIQ_DRIVER_EXPORT __declspec(dllexport)
+#define ROBOTIQ_DRIVER_IMPORT __declspec(dllimport)
+#endif
+#ifdef ROBOTIQ_DRIVER_BUILDING_DLL
+#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_EXPORT
+#else
+#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_IMPORT
+#endif
+#define ROBOTIQ_DRIVER_PUBLIC_TYPE ROBOTIQ_DRIVER_PUBLIC
+#define ROBOTIQ_DRIVER_LOCAL
+#else
+#define ROBOTIQ_DRIVER_EXPORT __attribute__((visibility("default")))
+#define ROBOTIQ_DRIVER_IMPORT
+#if __GNUC__ >= 4
+#define ROBOTIQ_DRIVER_PUBLIC __attribute__((visibility("default")))
+#define ROBOTIQ_DRIVER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define ROBOTIQ_DRIVER_PUBLIC
+#define ROBOTIQ_DRIVER_LOCAL
+#endif
+#define ROBOTIQ_DRIVER_PUBLIC_TYPE
+#endif
+
+#endif  // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_

src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp

@@ -12,7 +12,7 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
 
 // Connection and disconnection
 bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
-    std::lock_guard<std::mutex> lock(socketMutex_);
+    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ >= 0) {
         std::cerr << "Already connected. Disconnecting to reconnect.\n";
         disconnect();
@@ -48,7 +48,7 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
 }
 
 void Robotiq2fSocketAdapter::disconnect() {
-    std::lock_guard<std::mutex> lock(socketMutex_);
+    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ >= 0) {
         if (close(*sockfd_) == -1) {
             std::cerr << "Error closing socket: " << strerror(errno) << "\n";
@@ -61,7 +61,7 @@ void Robotiq2fSocketAdapter::disconnect() {
 
 // Utility methods
 bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
-    std::lock_guard<std::mutex> lock(socketMutex_);
+    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
         std::cerr << "Attempted to send command on a disconnected socket.\n";
         return false;
@@ -77,7 +77,7 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
 }
 
 std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
-    std::lock_guard<std::mutex> lock(socketMutex_);
+    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
         std::cerr << "Attempted to receive response on a disconnected socket.\n";
         return "";
@@ -162,12 +162,12 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
     for (const auto& [variable, value] : variableMap) {
         std::stringstream ss;
         ss << value;
-        cmd += " " + commandVariables.at(variable) + " " + ss.str();
+        cmd += " " + variable + " " + ss.str();
     }
     cmd += "\n";
 
     // Send the command and receive the response
-    std::lock_guard<std::mutex> lock(socketMutex_);
+    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
         throw std::runtime_error("Cannot set variables on a disconnected socket.");
         return false;
@@ -190,9 +190,9 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
     std::stringstream ss;
     ss << value;  // Convert the value to a string
 
-    std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n";
+    std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n";
 
-    std::lock_guard<std::mutex> lock(socketMutex_);
+    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
         throw std::runtime_error("Cannot set variables on a disconnected socket.");
         return false;
@@ -208,9 +208,9 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
 }
 
 int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
-    std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n";
+    std::string cmd = GET_COMMAND + " " + variable + "\n";
 
-    std::lock_guard<std::mutex> lock(socketMutex_);
+    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
         throw std::runtime_error("Cannot get variables on a disconnected socket.");
         return -1;

src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp

@@ -28,25 +28,17 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
         return CallbackReturn::ERROR;
     }
 
-    // Loading paramters from Hardware info and .yaml file
-       auto node = this->get_node();
-    node->declare_parameter("min_position", 0.0);
-    node->declare_parameter("max_position", 0.14);
-    node->declare_parameter("min_speed", 0.02);
-    node->declare_parameter("max_speed", 0.15);
-    node->declare_parameter("min_force", 20.0);
-    node->declare_parameter("max_force", 235.0);
     try
     {
         robot_ip_ = info_.hardware_parameters.at("robot_ip");
         robot_port_ = std::stod(info_.hardware_parameters.at("robot_port"));
         use_mock_hardware_ = std::stod(info_.hardware_parameters.at("use_fake_hardware"));
-        node->get_parameter("min_position", min_position_);
+        min_position_ = std::stod(info_.hardware_parameters.at("min_position"));
-        node->get_parameter("max_position", max_position_);
+        max_position_ = std::stod(info_.hardware_parameters.at("max_position"));
-        node->get_parameter("min_speed", min_speed_);
+        min_speed_ = std::stod(info_.hardware_parameters.at("min_speed"));
-        node->get_parameter("max_speed", max_speed_);
+        max_speed_ = std::stod(info_.hardware_parameters.at("max_speed"));
-        node->get_parameter("min_force", min_force_);
+        min_force_ = std::stod(info_.hardware_parameters.at("min_force"));
-        node->get_parameter("max_force", max_force_);
+        max_force_ = std::stod(info_.hardware_parameters.at("max_force"));
     }
     catch (const std::exception& e)
     {
@@ -101,7 +93,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
     return CallbackReturn::SUCCESS;
 }
 
-hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
+hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state)
 {
     RCLCPP_DEBUG(logger_, "on_configure");
     try
@@ -307,7 +299,7 @@ void RobotiqGripperHardwareInterface::background_task()
 {
     while (communication_thread_is_running_.load())
     {
-    std::lock_guard<std::mutex> guard(resource_mutex);
+        std::lock_guard<std::mutex> guard(resource_mutex_);
         try
         {
             // Re-activate the gripper
@@ -325,7 +317,8 @@ void RobotiqGripperHardwareInterface::background_task()
             int scaled_speed = write_speed_.load();
             int scaled_force = write_force_.load();
 
-      if (!socket_adapter_->move(scaled_position, scaled_speed, scaled_force)) {
+            auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force);
+            if (!std::get<0>(result)) {
                 throw std::runtime_error("Failed to send move command to Robotiq gripper.");
             }
 
@@ -364,7 +357,7 @@ double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
     return std::max(min_position_, std::min(scaled_position, max_position_));
 }
 
-uint8_t RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
+int RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
     if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
         throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
     }
@@ -372,7 +365,7 @@ uint8_t RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
     return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
 }
 
-uint8_t RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
+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.");
     }
@@ -380,7 +373,7 @@ uint8_t RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
     return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
 }
 
-uint8_t RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
+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.");
     }
@@ -392,4 +385,4 @@ uint8_t RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
 
 #include "pluginlib/class_list_macros.hpp"
 
-PLUGINLIB_EXPORT_CLASS(robotiq_2f_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)
+PLUGINLIB_EXPORT_CLASS(robotiq::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)