hardwareinterface doesnt export the command interfaces correctly

Issue probably in the Robotiq2fSocketAdapter.cpp since the mock hardware socket works fine

fixed some issues in 85 urdf

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

@@ -16,7 +16,7 @@ def generate_launch_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_urdf.rviz")
+    default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_robot.rviz")
 
     args = []
     args.append(

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

@@ -10,6 +10,7 @@
         robot_port
         min_position
         max_position
+        max_angle
         min_speed
         max_speed
         min_force
@@ -19,8 +20,9 @@
             <!-- 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="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}">
@@ -29,25 +31,31 @@
                     <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>
-                </xacro:unless>
             </hardware>
 
             <!-- Joint interfaces -->
             <!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
-            <joint name="${prefix}finger_joint">
+            <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>
-                <state_interface name="velocity"/>
             </joint>
             <!-- When simulating we need to include the rest of the gripper joints -->
             <xacro:if value="${use_fake_hardware}">

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

@@ -237,13 +237,14 @@
             </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}robotiq_85_left_knuckle_joint" type="revolute">
+        <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" />
@@ -251,57 +252,84 @@
             <limit lower="0.0" upper="0.8" velocity="0.5" effort="50" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_right_knuckle_joint" type="revolute">
+        <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}robotiq_85_left_knuckle_joint" multiplier="-1" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_left_finger_joint" type="fixed">
+        <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}robotiq_85_right_finger_joint" type="fixed">
+        <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}robotiq_85_left_inner_knuckle_joint" type="continuous">
+        <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}robotiq_85_left_knuckle_joint" />
+            <mimic joint="${prefix}finger_joint" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_right_inner_knuckle_joint" type="continuous">
+        <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}robotiq_85_left_knuckle_joint" multiplier="-1" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_left_finger_tip_joint" type="continuous">
+        <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}robotiq_85_left_knuckle_joint" multiplier="-1" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_right_finger_tip_joint" type="continuous">
+        <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}robotiq_85_left_knuckle_joint" />
+            <mimic joint="${prefix}finger_joint" />
         </joint>
+
+        <!-- Pseudo Link and Joint for gripper gap -->
+        <link name="${prefix}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>
+
+        <joint name="base_to_dummy" type="fixed">
+            <parent link="${prefix}robotiq_140_base_link"/>
+            <child link="dummy_link"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Adjust origin as needed -->
+        </joint>
+
+        <joint name="gripper_gap" type="prismatic">
+            <parent link="${prefix}dummy_link"/>
+            <child link="${prefix}robotiq_85_right_finger_tip_link"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Set origin appropriately -->
+            <axis xyz="0 1 0"/>  <!-- Axis of movement; adjust according to actual alignment -->
+            <limit lower="0.0" upper="0.085" effort="235" velocity="0.15"/>
+        </joint>
+
     </xacro:macro>
 </robot>

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

@@ -97,7 +97,7 @@ private:
             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;
+        // std::cout << "Reached position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
     }
 };
 } // end robotiq_interface

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

@@ -161,7 +161,7 @@ public:
     *         activation command fails, or if the gripper does not reach the expected state within
     *         a reasonable timeframe.
     */
-    void activate(bool autoCalibrate = true) override;
+    void activate(bool autoCalibrate = false) override;
     /**
     * Performs auto-calibration of the gripper.
     *
@@ -354,7 +354,7 @@ private:
 
     // Constants buffer sizes
     const size_t BUFFER_SIZE = 1024; // Adjust as necessary
-    const int MAX_RETRIES = 2000;
+    const int MAX_RETRIES = 200;
     const int RETRY_DELAY_MS = 50;
 
     // Logging

src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp

@@ -16,14 +16,14 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
 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";
+        // 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";
+        // std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
         RCLCPP_ERROR(logger_, "Socket creation failed!");
         return false;
     }
@@ -37,7 +37,7 @@ bool Robotiq2fSocketAdapter::connecting(const std::string& hostname, int port) {
     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";
+        // std::cerr << "Invalid address: " << hostname << "\n";
         RCLCPP_ERROR(logger_, "Invalid address: %s", hostname.c_str());
         close(*sockfd_);
         *sockfd_ = -1;
@@ -45,7 +45,7 @@ bool Robotiq2fSocketAdapter::connecting(const std::string& hostname, int port) {
     }
 
     if (connect(*sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) != 0) {
-        std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
+        // 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;
@@ -63,10 +63,10 @@ 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";
+            // std::cerr << "Error closing socket: " << strerror(errno) << "\n";
             RCLCPP_ERROR(logger_, "Error closing socket:  %s", strerror(errno));
         } else {
-            std::cout << "Disconnected successfully.\n";
+            // std::cout << "Disconnected successfully.\n";
             RCLCPP_INFO(logger_, "Disconnected successfully.");
         }
         *sockfd_ = -1;
@@ -77,13 +77,14 @@ void Robotiq2fSocketAdapter::disconnect() {
 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";
+        // 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));
-        std::cerr << "Failed to send command: " << strerror(errno) << "\n";
         return false;
     }
 
@@ -93,7 +94,8 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
 std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
     std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
-        std::cerr << "Attempted to receive response on a disconnected socket.\n";
+        // std::cerr << "Attempted to send command on a disconnected socket.\n";
+        RCLCPP_ERROR(logger_, "Attempted to send command on a disconnected socket.");
         return "";
     }
 
@@ -111,30 +113,37 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
         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";
+            // 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";
+                // 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";
+            // std::cerr << "Connection closed by peer.\n";
+            RCLCPP_ERROR(logger_, "Connection closed by peer.");
             return "";
         } 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;
+            } else if (result == "ack"){
+                complete_response = true;
             }
         }
     }
@@ -189,7 +198,7 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, uns
     }
 
     std::string response = receiveResponse();
-    RCLCPP_INFO(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    RCLCPP_DEBUG(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
     return isAck(response);
 }
 
@@ -216,7 +225,7 @@ 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());
+    RCLCPP_DEBUG(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
     return isAck(response);
 }
 
@@ -237,7 +246,7 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, uns
     }
 
     std::string response = receiveResponse();
-    RCLCPP_INFO(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    RCLCPP_DEBUG(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
     return isAck(response);
 }
 
@@ -258,7 +267,7 @@ 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());
+    RCLCPP_DEBUG(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
     return isAck(response);
 }
 
@@ -274,7 +283,7 @@ int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
         return -1; // Error sending command
     }
     std::string response = receiveResponse();
-    RCLCPP_INFO(logger_, "[getGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    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;
 
@@ -291,7 +300,8 @@ int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
     try {
         return std::stoi(value_str);
     } catch (const std::exception& ex) {
-        std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
+        // std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
+        RCLCPP_ERROR(logger_, "Failed to parse gripper response: %s",  ex.what());
         return -1;
     }
 }
@@ -345,9 +355,6 @@ std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int
     unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
     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, unsigned int> variableMap = {
         { CMD_POS, clippedPosition },
@@ -365,8 +372,6 @@ std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int
     unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
     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, unsigned int> variableMap = {
         { CMD_POS, clippedPosition },
@@ -421,8 +426,9 @@ void Robotiq2fSocketAdapter::auto_calibrate(bool log) {
     min_position_ = std::get<0>(result); // Update min position
 
     if (log) {
-        std::cout << "Gripper auto-calibrated to ["
+        // std::cout << "Gripper auto-calibrated to ["
-                  << min_position_ << ", " << max_position_ << "]\n";
+        //           << min_position_ << ", " << max_position_ << "]\n";
+        RCLCPP_INFO(logger_, "Gripper auto-calibrated to [%d, %d]", min_position_, max_position_);
     }
 }
 } // end robotiq_interface