From ad34edc818e0a47235647a7e1e1eccc1907fd400 Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 11 Apr 2024 09:57:43 +0200 Subject: [PATCH] extracted the tool communication launch added new Robotiq package for hardware interface with socket adapter --- .../robotiq_2f_interface/CMakeLists.txt | 28 ++ .../Robotiq2fSocketAdapter.hpp | 356 +++++++++++++++++ .../hardware_interface.hpp | 63 +++ .../robotiq_2f_interface/package.xml | 24 ++ .../src/Robotiq2fSocketAdapter.cpp | 369 ++++++++++++++++++ .../src/hardware_interface.cpp | 15 + .../launch/tool_communication.launch.py | 67 ++++ .../launch/ur_robotiq_control.launch.py | 16 +- 8 files changed, 930 insertions(+), 8 deletions(-) create mode 100644 src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt create mode 100644 src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp create mode 100644 src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp create mode 100644 src/robotiq_2f/robotiq_2f_interface/package.xml create mode 100644 src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp create mode 100644 src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp create mode 100644 src/ur_robotiq_description/launch/tool_communication.launch.py diff --git a/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt new file mode 100644 index 0000000..2f1bfeb --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(robotiq_2f_interface) + +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(rclcpp_lifecycle REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(pluginlib REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp new file mode 100644 index 0000000..2b8b280 --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp @@ -0,0 +1,356 @@ +// Robotiq2fSocketAdapter.hpp + +#ifndef ROBOTIQ2F_SOCKET_ADAPTER_HPP +#define ROBOTIQ2F_SOCKET_ADAPTER_HPP + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include // For close() +#include // For memset() +#include // For select() system call + +#include // For std::runtime_error +#include // For errno +#include + +// 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: + + Robotiq2fSocketAdapter() ; + ~Robotiq2fSocketAdapter(); + + // Connection management + /** + * Establishes a TCP connection to the Robotiq gripper. + * + * Attempts to open a socket and connect to the specified 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 connect(const std::string& hostname, int port); + /** + * 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(); + + + /** + * 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& variableMap); + bool setGripperVariables(const std::map& 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, 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 + bool isGripperActive(); + int getGripperPosition(); + int getGripperForce(); + int force(); + int speed(); + int position(); + + /** + * 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(); + + + // 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 = true); + /** + * 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 autoCalibrate(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(); + + // 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 move(int position, int speed, int force); + /** + * 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 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 _sockfd; + std::mutex socketMutex; // Mutex for socket access synchronization + + // bounds of the encoded gripper states + int _min_position = 0; + int _max_position = 255; + 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=2000); + + + /** + * 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. + */ + 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 +}; + +#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp new file mode 100644 index 0000000..5ed189e --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp @@ -0,0 +1,63 @@ +// hardware_interface.hpp + +#ifndef ROBOTIQ2F_HARDWARE_INTERFACE_HPP +#define ROBOTIQ2F_HARDWARE_INTERFACE_HPP + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +namespace robotiq_driver +{ +class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface +{ +public: + // Constructors + ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface(); + ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface(); + // ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr driver_factory); + + // Lifecycle Management + ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; + ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + // State and Command Interfaces + ROBOTIQ_DRIVER_PUBLIC std::vector export_state_interfaces() override; + ROBOTIQ_DRIVER_PUBLIC std::vector export_command_interfaces() override; + + // Data Access (Read/Write) + ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; + ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; + +protected: // Likely changes the access to protected from private + // Members for driver interaction + std::unique_ptr socket_adapter_; + // std::unique_ptr driver_factory_; + + // Background communication thread + std::thread communication_thread_; + std::atomic communication_thread_is_running_; + void background_task(); + + // Gripper State Tracking + double gripper_closed_pos_ = 0.0; + double gripper_position_ = 0.0; + double gripper_velocity_ = 0.0; + double gripper_position_command_ = 0.0; + // ... other internal state variables ... +}; +} + +#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_interface/package.xml b/src/robotiq_2f/robotiq_2f_interface/package.xml new file mode 100644 index 0000000..2b5bbdb --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/package.xml @@ -0,0 +1,24 @@ + + + + robotiq_2f_interface + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_lifecycle + hardware_interface + lifecycle_msgs + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp new file mode 100644 index 0000000..9d99ab7 --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp @@ -0,0 +1,369 @@ +#include "Robotiq2fSocketAdapter.hpp" + + +Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : _sockfd(new int(-1)) { +} + + +Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() { + disconnect(); // Ensure the socket is closed properly. +} + +// Connection and disconnection +bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) { + std::lock_guard lock(socketMutex); + if (*_sockfd >= 0) { + std::cerr << "Already connected. Disconnecting to reconnect.\n"; + disconnect(); + } + + int sock = socket(AF_INET, SOCK_STREAM, 0); + if (sock < 0) { + std::cerr << "Socket creation failed: " << strerror(errno) << "\n"; + 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"; + 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"; + close(*_sockfd); + *_sockfd = -1; + return false; + } + + return true; +} + +void Robotiq2fSocketAdapter::disconnect() { + std::lock_guard lock(socketMutex); + if (*_sockfd >= 0) { + if (close(*_sockfd) == -1) { + std::cerr << "Error closing socket: " << strerror(errno) << "\n"; + } else { + std::cout << "Disconnected successfully.\n"; + } + *_sockfd = -1; + } +} + +// Utility methods +bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) { + std::lock_guard lock(socketMutex); + if (*_sockfd < 0) { + 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) { + std::cerr << "Failed to send command: " << strerror(errno) << "\n"; + return false; + } + + return true; +} + +std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) { + std::lock_guard lock(socketMutex); + if (*_sockfd < 0) { + std::cerr << "Attempted to receive response on a disconnected socket.\n"; + 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); + + int activity = select(*_sockfd + 1, &readfds, nullptr, nullptr, &tv); + if (activity == -1) { + std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n"; + return ""; + } else if (activity == 0) { + std::cerr << "Timeout occurred while waiting for response.\n"; + 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"; + } + return ""; + } else if (bytes_read == 0) { + std::cerr << "Connection closed by peer.\n"; + 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; + } + } + } + + return result; +} + +bool Robotiq2fSocketAdapter::isAck(const std::string& data) { + return data == ACK_MESSAGE; +} + +int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) { + return std::max(min_value, std::min(value, max_value)); +} + +void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) { + int retries = 0; + int status = getGripperVariable(CMD_STA); + while (static_cast(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& variableMap) { + return setGripperVariables(reinterpret_cast&>(variableMap)); +} + +bool Robotiq2fSocketAdapter::setGripperVariables(const std::map& variableMap) { + // Build the command string + std::string cmd = SET_COMMAND; + for (const auto& [variable, value] : variableMap) { + std::stringstream ss; + ss << value; + cmd += " " + commandVariables.at(variable) + " " + ss.str(); + } + cmd += "\n"; + + // Send the command and receive the response + std::lock_guard lock(socketMutex); + 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(); + return isAck(response); +} + +bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, int value) { + return setGripperVariable(variable, static_cast(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 + " " + commandVariables.at(variable) + " " + ss.str() + "\n"; + + std::lock_guard lock(socketMutex); + 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(); + return isAck(response); +} + +int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) { + std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n"; + + std::lock_guard lock(socketMutex); + 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(); + + // Parse the response (assuming format: "variable_name value") + std::istringstream iss(response); + std::string var_name, value_str; + + if (!(iss >> var_name >> value_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; + 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, 1); // Activate the gripper + waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active + + if (autoCalibrate) { + auto_calibrate(); + } +} + +void Robotiq2fSocketAdapter::reset() { + setGripperVariable(CMD_ACT, 0); + setGripperVariable(CMD_ATR, 0); + waitForGripperStatus(GripperStatus::RESET); +} + + +void Robotiq2fSocketAdapter::deactivate() { + if (is_active()) { + reset(); + } +} + +// State subsscriber +bool Robotiq2fSocketAdapter::is_active(){ + GripperStatus status = static_cast(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 Robotiq2fSocketAdapter::move(int position, int speed, int force) { + int clippedPosition = clip_val(_min_position, position, _max_position); + int clippedSpeed = clip_val(_min_speed, speed, _max_speed); + int clippedForce = clip_val(_min_force, force, _max_force); + + // Create a map for gripper variables (similar to Python's OrderedDict) + std::map variableMap = { + { CMD_POS, clippedPosition }, + { CMD_SPE, clippedSpeed }, + { CMD_FOR, clippedForce }, + { CMD_GTO, 1 } + }; + + bool setResult = setGripperVariables(variableMap); + return std::make_tuple(setResult, clippedPosition); +} + +std::tuple Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) { + int clippedPosition = clip_val(min_position, position, max_position); + int clippedSpeed = clip_val(min_speed, speed, max_speed); + int clippedForce = clip_val(min_force, force, max_force); + + // Set gripper variables to initiate the move + std::map variableMap = { + { CMD_POS, clippedPosition }, + { CMD_SPE, clippedSpeed }, + { CMD_FOR, clippedForce }, + { CMD_GTO, 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."); + } + + // Wait until position is acknowledged: + while(getGripperVariable(CMD_PRE) != clippedPosition) { + 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(currentObjectStatus); + return std::make_tuple(finalPosition, finalStatus); +} + + +void Robotiq2fSocketAdapter::auto_calibrate(bool log=true) { + // Open in case we are holding an object + std::tuple 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"; + } +} diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp new file mode 100644 index 0000000..10c65aa --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp @@ -0,0 +1,15 @@ +#include + +ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface() + : gripper_adapter_(std::make_unique()) +{ + +} + +ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface() { + // 1. Disconnect from the gripper + gripper_adapter_->disconnect(); + + // 2. Clean up other resources if necessary + // ... +} \ No newline at end of file diff --git a/src/ur_robotiq_description/launch/tool_communication.launch.py b/src/ur_robotiq_description/launch/tool_communication.launch.py new file mode 100644 index 0000000..7394e16 --- /dev/null +++ b/src/ur_robotiq_description/launch/tool_communication.launch.py @@ -0,0 +1,67 @@ +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile +from launch_ros.substitutions import FindPackageShare + +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 + + +def launch_setup(context, *args, **kwargs): + # Initialize Arguments + robot_ip = LaunchConfiguration("robot_ip") + tool_device_name = LaunchConfiguration("tool_device_name") + tool_tcp_port = LaunchConfiguration("tool_tcp_port") + + + + tool_communication_node = Node( + package="ur_robot_driver", + 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, + } + ], + ) + + + + nodes_to_start = [tool_communication_node] + + return nodes_to_start + + +def generate_launch_description(): + # Declare Arguments + declared_arguments = [] + # UR specific arguments + declared_arguments.append( + DeclareLaunchArgument( + "robot_ip", description="IP address by which the robot can be reached." + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_device_name", + default_value="/tmp/ttyUR", + description="File descriptor that will be generated for the tool communication device. \ + The user has be be allowed to write to this location. \ + Only effective, if use_tool_communication is set to True.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "tool_tcp_port", + default_value="50006", + description="Remote port that will be used for bridging the tool's serial device. \ + Only effective, if use_tool_communication is set to True.", + ) + ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py index 6771f73..776bc8a 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py +++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py @@ -50,7 +50,7 @@ def launch_setup(context, *args, **kwargs): [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"] ) kinematics_params = PathJoinSubstitution( - [FindPackageShare(ur_description_package), "config", ur_type, "default_kinematics.yaml"] + [FindPackageShare(description_package), "config","ur_robotiq_calibration.yaml"] ) physical_params = PathJoinSubstitution( [FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"] @@ -348,18 +348,18 @@ def launch_setup(context, *args, **kwargs): ) nodes_to_start = [ + # tool_communication_node, control_node, ur_control_node, - robotiq_gripper_controller_spawner, - robotiq_gripper_joint_trajectory_controller_spawner, - robotiq_activation_controller_spawner, dashboard_client_node, - tool_communication_node, controller_stopper_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 return nodes_to_start @@ -505,7 +505,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "use_tool_communication", - default_value="false", + default_value="true", description="Only available for e series!", ) ) @@ -561,7 +561,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "tool_tcp_port", - default_value="54321", + default_value="63352", description="Remote port that will be used for bridging the tool's serial device. \ Only effective, if use_tool_communication is set to True.", ) @@ -569,7 +569,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "tool_voltage", - default_value="0", # 0 being a conservative value that won't destroy anything + default_value="24", # 0 being a conservative value that won't destroy anything description="Tool voltage that will be setup.", ) )