Niko/UR_Robotiq
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
NikoFeith 2024-04-30 20:09:11 +02:00
parent ec5f28fd76
commit dc82ec4787
11 changed files with 142 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py
View File

@ -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':

18
src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz
View File

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

2
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro
View File

@ -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">

2
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp
View File

@ -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
}

21
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp
View File

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

2
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp
View File

@ -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;

2
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp
View File

@ -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");

128
src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp
View File

@ -4,6 +4,7 @@
namespace robotiq_interface
{
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
RCLCPP_INFO(logger_, "Constructor");
}
@ -12,20 +13,24 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
}
// Connection and disconnection
bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
bool Robotiq2fSocketAdapter::connecting(const std::string& hostname, int port) {
std::lock_guard<std::mutex> lock(socket_mutex_);
if (*sockfd_ >= 0) {
std::cerr << "Already connected. Disconnecting to reconnect.\n";
RCLCPP_ERROR(logger_, "Already connected. Disconnecting to reconnect.");
disconnect();
}
int sock = socket(AF_INET, SOCK_STREAM, 0);
if (sock < 0) {
std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
RCLCPP_ERROR(logger_, "Socket creation failed!");
return false;
}
*sockfd_ = sock;
sockaddr_in serv_addr{};
memset(&serv_addr, 0, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
@ -33,19 +38,25 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) {
std::cerr << "Invalid address: " << hostname << "\n";
RCLCPP_ERROR(logger_, "Invalid address: %s", hostname.c_str());
close(*sockfd_);
*sockfd_ = -1;
return false;
}
if (::connect(*sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
if (connect(*sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) != 0) {
std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
RCLCPP_ERROR(logger_, "Connection to %s: %d failed!", hostname.c_str(), port);
close(*sockfd_);
*sockfd_ = -1;
return false;
}
else{
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<unsigned int>(clipped);
}
void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) {
@ -153,8 +166,31 @@ void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus)
// Gripper variable Getter and Setter
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, int>& variableMap) {
return setGripperVariables(reinterpret_cast<const std::map<std::string, double>&>(variableMap));
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, unsigned int>& variableMap) {
// Build the command string
std::string cmd = SET_COMMAND;
for (const auto& [variable, value] : variableMap) {
std::stringstream ss;
ss << value;
cmd += " " + variable + " " + ss.str();
}
cmd += "\n";
// Send the command and receive the response
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot set variables on a disconnected socket.");
return false;
}
if (!sendCommand(cmd)) {
throw std::runtime_error("[setGripperVariables] Sending command failed.");
return false;
}
std::string response = receiveResponse();
RCLCPP_INFO(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
return isAck(response);
}
bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, double>& variableMap) {
@ -168,7 +204,7 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
cmd += "\n";
// Send the command and receive the response
std::lock_guard<std::mutex> lock(socket_mutex_);
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot set variables on a disconnected socket.");
return false;
@ -180,20 +216,16 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
}
std::string response = receiveResponse();
RCLCPP_INFO(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
return isAck(response);
}
bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, int value) {
return setGripperVariable(variable, static_cast<double>(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<std::mutex> lock(socket_mutex_);
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot set variables on a disconnected socket.");
return false;
@ -205,26 +237,44 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
}
std::string response = receiveResponse();
RCLCPP_INFO(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
return isAck(response);
}
bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) {
std::stringstream ss;
ss << value; // Convert the value to a string
std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n";
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot set variables on a disconnected socket.");
return false;
}
if (!sendCommand(cmd)) {
throw std::runtime_error("[setGripperVariable] Sending command failed.");
return false; // Sending command failed
}
std::string response = receiveResponse();
RCLCPP_INFO(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
return isAck(response);
}
int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
std::string cmd = GET_COMMAND + " " + variable + "\n";
std::lock_guard<std::mutex> lock(socket_mutex_);
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot get variables on a disconnected socket.");
return -1;
}
if (!sendCommand(cmd)) {
throw std::runtime_error("[getGripperVariable] Sending command failed.");
return -1; // Error sending command
}
std::string response = receiveResponse();
// 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<unsigned int>(1)); // Activate the gripper
waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active
if (autoCalibrate) {
auto_calibrate();
}
RCLCPP_INFO(logger_, "Socket adapter activated.");
}
void Robotiq2fSocketAdapter::reset() {
setGripperVariable(CMD_ACT, 0);
setGripperVariable(CMD_ATR, 0);
setGripperVariable(CMD_ACT, static_cast<unsigned int>(0));
setGripperVariable(CMD_ATR, static_cast<unsigned int>(0));
waitForGripperStatus(GripperStatus::RESET);
}
@ -290,16 +341,19 @@ int Robotiq2fSocketAdapter::position(){
// Movement Methods
std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
int clippedPosition = clip_val(min_position_, position, max_position_);
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<std::string, int> variableMap = {
std::map<std::string, unsigned int> variableMap = {
{ CMD_POS, clippedPosition },
{ CMD_SPE, clippedSpeed },
{ CMD_FOR, clippedForce },
{ CMD_GTO, 1 }
{ CMD_GTO, static_cast<unsigned int>(1) }
};
bool setResult = setGripperVariables(variableMap);
@ -307,21 +361,23 @@ std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int
}
std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
int clippedPosition = clip_val(min_position_, position, max_position_);
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<std::string, int> variableMap = {
std::map<std::string, unsigned int> variableMap = {
{ CMD_POS, clippedPosition },
{ CMD_SPE, clippedSpeed },
{ CMD_FOR, clippedForce },
{ CMD_GTO, 1 }
{ CMD_GTO, static_cast<unsigned int>(1) }
};
bool setResult = setGripperVariables(variableMap);
if (!setResult) {
// Handle the error case, e.g., throw an exception
throw std::runtime_error("Failed to set variables for move.");
RCLCPP_ERROR(logger_, "Failed to set variables for move.");
}
// Wait until position is acknowledged:

21
src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
View File

@ -53,7 +53,7 @@ RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
{
RCLCPP_DEBUG(logger_, "on_init");
RCLCPP_INFO(logger_, "on_init");
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
{
@ -65,6 +65,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
if (info.hardware_parameters.count(robot_ip_parameter_name) > 0) {
robot_ip_ = info.hardware_parameters.at(robot_ip_parameter_name);
} else {
RCLCPP_DEBUG(logger_, "Falling back on default ip!");
robot_ip_ = robot_ip_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: Robot IP, Value: %s", robot_ip_.c_str());
@ -209,7 +210,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state)
{
RCLCPP_DEBUG(logger_, "on_configure");
RCLCPP_INFO(logger_, "on_configure");
try
{
if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS)
@ -218,11 +219,6 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure
}
configureAdapter(use_mock_hardware_);
if (!socket_adapter_->connect(robot_ip_, robot_port_))
{
RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
return CallbackReturn::ERROR;
}
}
catch (const std::exception& e)
{
@ -235,7 +231,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
{
RCLCPP_DEBUG(logger_, "on_activate");
RCLCPP_INFO(logger_, "on_activate");
// set some default values for joints
if (std::isnan(gripper_gap_))
@ -249,10 +245,15 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(
// Activate the gripper.
try
{
RCLCPP_INFO(logger_, "Starting Socket connection!");
if (!socket_adapter_->connecting(robot_ip_, robot_port_))
{
RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
return CallbackReturn::ERROR;
}
socket_adapter_->deactivate();
socket_adapter_->activate();
communication_thread_is_running_.store(true);
communication_thread_ = std::thread([this] { this->background_task(); });
RCLCPP_INFO(logger_, "Background task thread started.");
@ -299,7 +300,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivat
std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface::export_state_interfaces()
{
RCLCPP_DEBUG(logger_, "export_state_interfaces");
RCLCPP_INFO(logger_, "export_state_interfaces");
std::vector<hardware_interface::StateInterface> state_interfaces;

6
src/ur_robotiq_description/package.xml
View File

@ -23,9 +23,9 @@
<exec_depend>ur_description</exec_depend>
<exec_depend>ur_robot_driver</exec_depend>
<exec_depend>ur_controllers</exec_depend>
<exec_depend>robotiq_description</exec_depend>
<exec_depend>robotiq_driver</exec_depend>
<exec_depend>robotiq_controllers</exec_depend>
<exec_depend>robotiq_2f_description</exec_depend>
<exec_depend>robotiq_2f_interface</exec_depend>
<exec_depend>robotiq_2f_controllers</exec_depend>
<exec_depend>gripper_controllers</exec_depend>
<exec_depend>joint_state_broadcaster</exec_depend>

6
src/ur_robotiq_moveit_config/package.xml
View File

@ -45,9 +45,9 @@
<exec_depend>ur_description</exec_depend>
<exec_depend>ur_robot_driver</exec_depend>
<exec_depend>ur_controllers</exec_depend>
<exec_depend>robotiq_description</exec_depend>
<exec_depend>robotiq_driver</exec_depend>
<exec_depend>robotiq_controllers</exec_depend>
<exec_depend>robotiq_2f_description</exec_depend>
<exec_depend>robotiq_2f_interface</exec_depend>
<exec_depend>robotiq_2f_controllers</exec_depend>
<exec_depend>gripper_controllers</exec_depend>
<depend>control_msgs</depend>