From a31be7d77e19d247711f4c330dbb767cb0411266 Mon Sep 17 00:00:00 2001 From: NikoFeith Date: Thu, 2 May 2024 13:55:10 +0200 Subject: [PATCH] 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 --- .../launch/view_gripper.launch.py | 2 +- .../rviz/{view_urdf.rviz => view_robot.rviz} | 0 .../urdf/robotiq_2f_85.ros2_control.xacro | 34 ++++++---- .../urdf/robotiq_2f_85_macro.urdf.xacro | 54 ++++++++++++---- .../MockRobotiq2fSocketAdapter.hpp | 2 +- .../Robotiq2fSocketAdapter.hpp | 4 +- .../src/Robotiq2fSocketAdapter.cpp | 62 ++++++++++--------- 7 files changed, 100 insertions(+), 58 deletions(-) rename src/robotiq_2f/robotiq_2f_description/rviz/{view_urdf.rviz => view_robot.rviz} (100%) diff --git a/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py index 9c58ccf..d259367 100644 --- a/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py +++ b/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( diff --git a/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz b/src/robotiq_2f/robotiq_2f_description/rviz/view_robot.rviz similarity index 100% rename from src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz rename to src/robotiq_2f/robotiq_2f_description/rviz/view_robot.rviz diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro index 1b51269..0649b82 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro +++ b/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 @@ -18,9 +19,10 @@ - - mock_components/GenericSystem - ${fake_sensor_commands} + + robotiq_2f_interface/RobotiqGripperHardwareInterface + ${use_fake_hardware} + ${fake_sensor_commands} 0.0 @@ -29,25 +31,31 @@ ${robot_ip} ${robot_port} ${use_fake_hardware} - 1.0 - 0.5 - ${min_position} - ${max_position} - ${min_speed} - ${max_speed} - ${min_force} - ${max_force} + 1.0 + 0.5 + ${min_position} + ${max_position} + ${max_angle} + ${min_speed} + ${max_speed} + ${min_force} + ${max_force} - + + + 0.0 + + + + 0.7929 - diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro index f61540b..afdd6fb 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro @@ -237,13 +237,14 @@ + - + @@ -251,57 +252,84 @@ - + - + - + - + - + - + - + - + - + - + - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 99b2677..4a3aaa8 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 @@ -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 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 bee0547..741ea33 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 @@ -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 diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp index f73d733..21ddbef 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp +++ b/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 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 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 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 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; } } } @@ -167,7 +176,7 @@ void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) // Gripper variable Getter and Setter bool Robotiq2fSocketAdapter::setGripperVariables(const std::map& variableMap) { - // Build the command string + // Build the command string std::string cmd = SET_COMMAND; for (const auto& [variable, value] : variableMap) { std::stringstream ss; @@ -189,7 +198,7 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map 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 variableMap = { { CMD_POS, clippedPosition }, @@ -365,8 +372,6 @@ std::tuple 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 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 [" - << min_position_ << ", " << max_position_ << "]\n"; + // std::cout << "Gripper auto-calibrated to [" + // << min_position_ << ", " << max_position_ << "]\n"; + RCLCPP_INFO(logger_, "Gripper auto-calibrated to [%d, %d]", min_position_, max_position_); } } } // end robotiq_interface \ No newline at end of file