From dc82ec47873a33522962725a0ee3f804e8e4400d Mon Sep 17 00:00:00 2001 From: NikoFeith Date: Tue, 30 Apr 2024 20:09:11 +0200 Subject: [PATCH] problems with ack message -> it is not in the right format --- .../launch/robotiq_control.launch.py | 7 +- .../rviz/view_urdf.rviz | 18 +-- .../urdf/robotiq_2f_140_macro.urdf.xacro | 2 +- .../MockRobotiq2fSocketAdapter.hpp | 2 +- .../Robotiq2fSocketAdapter.hpp | 21 +-- .../SocketAdapterBase.hpp | 2 +- .../hardware_interface.hpp | 2 +- .../src/Robotiq2fSocketAdapter.cpp | 130 +++++++++++++----- .../src/hardware_interface.cpp | 21 +-- src/ur_robotiq_description/package.xml | 6 +- src/ur_robotiq_moveit_config/package.xml | 6 +- 11 files changed, 142 insertions(+), 75 deletions(-) diff --git a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py index 027d6ad..4eac23f 100644 --- a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py +++ b/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': diff --git a/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz b/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz index 4da7e00..8b3a417 100644 --- a/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz +++ b/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: 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 - X: 1990 - Y: 27 + Width: 1200 + X: 70 + Y: 60 diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro index fac5c64..380da23 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro +++ b/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"> diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp index ae3ae9e..99b2677 100644 --- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp +++ b/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 } 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 index 8b952e0..bee0547 100644 --- 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 @@ -25,6 +25,8 @@ #include // For errno #include +#include + #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& variableMap); + bool setGripperVariables(const std::map& variableMap); bool setGripperVariables(const std::map& 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 RETRY_DELAY_MS = 20; + const int MAX_RETRIES = 2000; + const int RETRY_DELAY_MS = 50; + + // Logging + const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqSocketAdapter"); }; } // end robotiq_interface diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp index e7282a8..d93f82d 100644 --- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp +++ b/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; 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 index f0cafa2..165ca57 100644 --- 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 @@ -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"); diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp index 435ed69..f73d733 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp +++ b/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 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; } - - return true; + else{ + RCLCPP_INFO(logger_, "Connection to %s: %d established!", hostname.c_str(), port); + return true; + } + RCLCPP_ERROR(logger_, "Unknown connection error in socket adapter connecting!"); + return false; } void Robotiq2fSocketAdapter::disconnect() { @@ -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) { - return std::max(min_value, std::min(value, max_value)); +unsigned int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) { + int clipped = std::max(min_value, std::min(value, max_value)); + return static_cast(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& 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 += " " + 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& variableMap) { @@ -168,7 +204,7 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map 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(value)); // Convert int to double -} - -bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) { +bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, unsigned int value) { std::stringstream ss; ss << value; // Convert the value to a string std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n"; - std::lock_guard 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 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(); - - // Parse the response (assuming format: "variable_name value") + RCLCPP_INFO(logger_, "[getGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str()); 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(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_ATR, 0); + setGripperVariable(CMD_ACT, static_cast(0)); + setGripperVariable(CMD_ATR, static_cast(0)); waitForGripperStatus(GripperStatus::RESET); } @@ -290,16 +341,19 @@ int Robotiq2fSocketAdapter::position(){ // 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_); + unsigned int clippedPosition = clip_val(min_position_, position, max_position_); + unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_); + unsigned int clippedForce = clip_val(min_force_, force, max_force_); + + RCLCPP_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 variableMap = { + std::map variableMap = { { CMD_POS, clippedPosition }, { CMD_SPE, clippedSpeed }, { CMD_FOR, clippedForce }, - { CMD_GTO, 1 } + { CMD_GTO, static_cast(1) } }; bool setResult = setGripperVariables(variableMap); @@ -307,21 +361,23 @@ std::tuple Robotiq2fSocketAdapter::move(int position, int speed, int } 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_); + unsigned int clippedPosition = clip_val(min_position_, position, max_position_); + unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_); + unsigned int clippedForce = clip_val(min_force_, force, max_force_); + + RCLCPP_INFO(logger_, "Clipped Position: %d, Clipped Speed: %d, Clipped Force: %d", clippedPosition, clippedSpeed, clippedForce); // Set gripper variables to initiate the move - std::map variableMap = { + std::map variableMap = { { CMD_POS, clippedPosition }, { CMD_SPE, clippedSpeed }, { CMD_FOR, clippedForce }, - { CMD_GTO, 1 } + { CMD_GTO, static_cast(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: diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp index 0c7039f..cf9a6ff 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp +++ b/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 RobotiqGripperHardwareInterface::export_state_interfaces() { - RCLCPP_DEBUG(logger_, "export_state_interfaces"); + RCLCPP_INFO(logger_, "export_state_interfaces"); std::vector state_interfaces; diff --git a/src/ur_robotiq_description/package.xml b/src/ur_robotiq_description/package.xml index f3faf4b..c9cc673 100644 --- a/src/ur_robotiq_description/package.xml +++ b/src/ur_robotiq_description/package.xml @@ -23,9 +23,9 @@ ur_description ur_robot_driver ur_controllers - robotiq_description - robotiq_driver - robotiq_controllers + robotiq_2f_description + robotiq_2f_interface + robotiq_2f_controllers gripper_controllers joint_state_broadcaster diff --git a/src/ur_robotiq_moveit_config/package.xml b/src/ur_robotiq_moveit_config/package.xml index e9ce5db..8b32640 100644 --- a/src/ur_robotiq_moveit_config/package.xml +++ b/src/ur_robotiq_moveit_config/package.xml @@ -45,9 +45,9 @@ ur_description ur_robot_driver ur_controllers - robotiq_description - robotiq_driver - robotiq_controllers + robotiq_2f_description + robotiq_2f_interface + robotiq_2f_controllers gripper_controllers control_msgs