problems with ack message -> it is not in the right format

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

@@ -6,7 +6,7 @@ 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 = []
@@ -109,12 +109,14 @@ def launch_setup(context, *args, **kwargs):
             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(
@@ -139,12 +141,14 @@ def launch_setup(context, *args, **kwargs):
             "--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):
@@ -157,6 +161,7 @@ def launch_setup(context, *args, **kwargs):
                           "--controller-manager",
                           "/controller_manager",
                       ] + inactive_flags,
+            output='screen',
         )
 
     if use_forward_controller.perform(context) == 'false':

src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz

@@ -8,7 +8,7 @@ Panels:
         - /Status1
         - /RobotModel1
       Splitter Ratio: 0.5
-    Tree Height: 719
+    Tree Height: 555
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -188,7 +188,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 1.321521282196045
+      Distance: 1.01347017288208
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -203,18 +203,18 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.30539846420288086
+      Pitch: 0.3253982663154602
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 1.455397367477417
+      Yaw: 1.3853975534439087
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1016
+  Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -223,6 +223,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1850
+  Width: 1200
-  X: 1990
+  X: 70
-  Y: 27
+  Y: 60

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

@@ -8,7 +8,7 @@
         include_ros2_control:=true
         use_fake_hardware:=false
         fake_sensor_commands:=false
-        robot_ip:=192.168.1.1
+        robot_ip:=192.168.1.223
         robot_port:=63352">
 
 

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

@@ -22,7 +22,7 @@ public:
         }
     }
 
-    bool connect(const std::string& hostname, int port) override {
+    bool connecting(const std::string& hostname, int port) override {
         isConnected.store(true);
         return true; // Simulate successful connection
     }

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

@@ -25,6 +25,8 @@
 #include <cerrno> // For errno
 #include <iostream>
 
+#include <rclcpp/rclcpp.hpp>
+
 #include "robotiq_2f_interface/SocketAdapterBase.hpp"
 
 namespace robotiq_interface
@@ -55,7 +57,7 @@ public:
     /**
      * Establishes a TCP connection to the Robotiq gripper.
      *
-     * Attempts to open a socket and connect to the specified hostname and port. If the adapter
+     * 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.
@@ -64,7 +66,7 @@ public:
      *
      * @throws std::runtime_error if socket creation fails.
      */
-    bool connect(const std::string& hostname, int port) override;
+    bool connecting(const std::string& hostname, int port) override;
     /**
     * Closes the connection to the Robotiq gripper.
     *
@@ -88,7 +90,7 @@ public:
     *
     * @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
     */
-    bool setGripperVariables(const std::map<std::string, int>& variableMap);
+    bool setGripperVariables(const std::map<std::string, unsigned int>& variableMap);
     bool setGripperVariables(const std::map<std::string, double>& variableMap);
 
     /**
@@ -104,7 +106,7 @@ public:
     *
     * @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
     */
-    bool setGripperVariable(const std::string& variable, int value);
+    bool setGripperVariable(const std::string& variable, unsigned int value);
     bool setGripperVariable(const std::string& variable, double value);
 
     /**
@@ -277,7 +279,7 @@ private:
     *
     * @throws std::runtime_error if attempted on a disconnected socket or if a select() call fails.
     */
-    std::string receiveResponse(int timeout_ms=2000);
+    std::string receiveResponse(int timeout_ms=2);
 
 
     /**
@@ -304,7 +306,7 @@ private:
     * @param max_value The maximum allowable value for the variable.
     * @return The clipped value, constrained within the specified min and max bounds.
     */
-    int clip_val(int min_value, int variable, int max_value);
+    unsigned int clip_val(int min_value, int variable, int max_value);
     /**
     * Blocks until the gripper reaches a specified status.
     *
@@ -352,8 +354,11 @@ private:
 
     // Constants buffer sizes
     const size_t BUFFER_SIZE = 1024; // Adjust as necessary
-    const int MAX_RETRIES = 5;
+    const int MAX_RETRIES = 2000;
-    const int RETRY_DELAY_MS = 20;
+    const int RETRY_DELAY_MS = 50;
+
+    // Logging
+    const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqSocketAdapter");
 };
 } // end robotiq_interface
 

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

@@ -12,7 +12,7 @@ class SocketAdapterBase {
 public:
     virtual ~SocketAdapterBase() = default;
 
-    virtual bool connect(const std::string& hostname, int port) = 0;
+    virtual bool connecting(const std::string& hostname, int port) = 0;
     virtual void disconnect() = 0;
     virtual int force() = 0;
     virtual int speed() = 0;

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

@@ -111,7 +111,7 @@ protected: // Likely changes the access to protected from private
     double max_force_;
 
     // loop time
-    const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
+    const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 2 };
 
     // Logger
     const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");

src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp

@@ -4,6 +4,7 @@
 namespace robotiq_interface
 {
 Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
+RCLCPP_INFO(logger_, "Constructor");
 }
 
 
@@ -12,20 +13,24 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
 }
 
 // Connection and disconnection
-bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
+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;
@@ -33,19 +38,25 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int 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) {
+    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{
-    return true;
+        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() {
@@ -53,8 +64,10 @@ void Robotiq2fSocketAdapter::disconnect() {
     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;
     }
@@ -67,9 +80,9 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
         std::cerr << "Attempted to send command on a disconnected socket.\n";
         return false;
     }
-
     ssize_t result = send(*sockfd_, cmd.c_str(), cmd.length(), 0);
     if (result < 0) {
+        RCLCPP_ERROR(logger_, "Failed to send command: %s", strerror(errno));
         std::cerr << "Failed to send command: " << strerror(errno) << "\n";
         return false;
     }
@@ -103,7 +116,6 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
             std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
             return "";
         } else if (activity == 0) {
-            std::cerr << "Timeout occurred while waiting for response.\n";
             return "";
         }
 
@@ -119,7 +131,7 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
         } else {
             buffer[bytes_read] = '\0';
             result += buffer;
-
+            RCLCPP_INFO(logger_, "Result: %s", result.c_str());
             // Check if we have a complete response:
             if (result.find("\n") != std::string::npos) {
                 complete_response = true;
@@ -134,8 +146,9 @@ bool Robotiq2fSocketAdapter::isAck(const std::string& data) {
     return data == ACK_MESSAGE;
 }
 
-int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
+unsigned int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
-    return std::max(min_value, std::min(value, max_value));
+    int clipped = std::max(min_value, std::min(value, max_value));
+    return static_cast<unsigned int>(clipped);
 }
 
 void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) {
@@ -153,8 +166,31 @@ void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus)
 
 
 // Gripper variable Getter and Setter
-bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, int>& variableMap) {
+bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, unsigned int>& variableMap) {
-    return setGripperVariables(reinterpret_cast<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_INFO(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) {
@@ -168,7 +204,7 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
     cmd += "\n";
 
     // Send the command and receive the response
-    std::lock_guard<std::mutex> lock(socket_mutex_);
+
     if (*sockfd_ < 0) {
         throw std::runtime_error("Cannot set variables on a disconnected socket.");
         return false;
@@ -180,20 +216,16 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
     }
 
     std::string response = receiveResponse();
+    RCLCPP_INFO(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
     return isAck(response);
 }
 
-bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, int value) {
+bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, unsigned int value) {
-    return setGripperVariable(variable, static_cast<double>(value)); // Convert int to double
-}
-
-bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) {
     std::stringstream ss;
     ss << value;  // Convert the value to a string
 
     std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n";
 
-    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
         throw std::runtime_error("Cannot set variables on a disconnected socket.");
         return false;
@@ -205,26 +237,44 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
     }
 
     std::string response = receiveResponse();
+    RCLCPP_INFO(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_INFO(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";
 
-    std::lock_guard<std::mutex> lock(socket_mutex_);
     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_INFO(logger_, "[getGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
-    // Parse the response (assuming format: "variable_name value")
     std::istringstream iss(response);
     std::string var_name, value_str;
 
@@ -249,17 +299,18 @@ int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
 // Activation, deactivation and reset of the Gripper
 void Robotiq2fSocketAdapter::activate(bool autoCalibrate) {
     reset(); // Always start with a reset to ensure a known state
-    setGripperVariable(CMD_ACT, 1); // Activate the gripper
+    setGripperVariable(CMD_ACT, static_cast<unsigned int>(1)); // Activate the gripper
     waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active
 
     if (autoCalibrate) {
         auto_calibrate();
     }
+    RCLCPP_INFO(logger_, "Socket adapter activated.");
 }
 
 void Robotiq2fSocketAdapter::reset() {
-    setGripperVariable(CMD_ACT, 0);
+    setGripperVariable(CMD_ACT, static_cast<unsigned int>(0));
-    setGripperVariable(CMD_ATR, 0);
+    setGripperVariable(CMD_ATR, static_cast<unsigned int>(0));
     waitForGripperStatus(GripperStatus::RESET);
 }
 
@@ -290,16 +341,19 @@ int Robotiq2fSocketAdapter::position(){
 
 // Movement Methods
 std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
-    int clippedPosition = clip_val(min_position_, position, max_position_);
+    unsigned int clippedPosition = clip_val(min_position_, position, max_position_);
-    int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
+    unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
-    int clippedForce = clip_val(min_force_, force, max_force_);
+    unsigned int clippedForce = clip_val(min_force_, force, max_force_);
+
+    RCLCPP_INFO(logger_, "Clipped Position: %d, Clipped Speed: %d, Clipped Force: %d", clippedPosition, clippedSpeed, clippedForce);
+
 
     // Create a map for gripper variables (similar to Python's OrderedDict)
-    std::map<std::string, int> variableMap = {
+    std::map<std::string, unsigned int> variableMap = {
         { CMD_POS, clippedPosition },
         { CMD_SPE, clippedSpeed },
         { CMD_FOR, clippedForce },
-        { CMD_GTO, 1 }
+        { CMD_GTO, static_cast<unsigned int>(1) }
     };
 
     bool setResult = setGripperVariables(variableMap);
@@ -307,21 +361,23 @@ std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int
 }
 
 std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
-    int clippedPosition = clip_val(min_position_, position, max_position_);
+    unsigned int clippedPosition = clip_val(min_position_, position, max_position_);
-    int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
+    unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
-    int clippedForce = clip_val(min_force_, force, max_force_);
+    unsigned int clippedForce = clip_val(min_force_, force, max_force_);
+
+    RCLCPP_INFO(logger_, "Clipped Position: %d, Clipped Speed: %d, Clipped Force: %d", clippedPosition, clippedSpeed, clippedForce);
 
     // Set gripper variables to initiate the move
-    std::map<std::string, int> variableMap = {
+    std::map<std::string, unsigned int> variableMap = {
         { CMD_POS, clippedPosition },
         { CMD_SPE, clippedSpeed },
         { CMD_FOR, clippedForce },
-        { CMD_GTO, 1 }
+        { CMD_GTO, static_cast<unsigned int>(1) }
     };
     bool setResult = setGripperVariables(variableMap);
     if (!setResult) {
         // Handle the error case, e.g., throw an exception
-        throw std::runtime_error("Failed to set variables for move.");
+        RCLCPP_ERROR(logger_, "Failed to set variables for move.");
     }
 
     // Wait until position is acknowledged:

src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp

@@ -53,7 +53,7 @@ RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
 
 hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
 {
-    RCLCPP_DEBUG(logger_, "on_init");
+    RCLCPP_INFO(logger_, "on_init");
 
     if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
     {
@@ -65,6 +65,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
         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());
@@ -209,7 +210,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
 
 hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state)
 {
-    RCLCPP_DEBUG(logger_, "on_configure");
+    RCLCPP_INFO(logger_, "on_configure");
     try
     {
         if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS)
@@ -218,11 +219,6 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure
         }
 
         configureAdapter(use_mock_hardware_);
-        if (!socket_adapter_->connect(robot_ip_, robot_port_))
-        {
-            RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
-            return CallbackReturn::ERROR;
-        }
     }
     catch (const std::exception& e)
     {
@@ -235,7 +231,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure
 
 hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
 {
-    RCLCPP_DEBUG(logger_, "on_activate");
+    RCLCPP_INFO(logger_, "on_activate");
 
     // set some default values for joints
     if (std::isnan(gripper_gap_))
@@ -249,10 +245,15 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(
     // 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.");
@@ -299,7 +300,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivat
 
 std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces()
 {
-    RCLCPP_DEBUG(logger_, "export_state_interfaces");
+    RCLCPP_INFO(logger_, "export_state_interfaces");
 
     std::vector<hardware_interface::StateInterface> state_interfaces;
 

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_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>