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

debugging

Browse Source
This commit is contained in:
Niko Feith 2024-04-11 18:28:14 +02:00
parent 625c5082b0
commit 719502e631
24 changed files with 1400 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
UR_Robotiq.humble.repos
View File

@ -7,14 +7,6 @@ repositories:
type: git
url: https://github.com/NikoFeith/Universal_Robots_ROS2_Driver.git
version: humble
serial:
type: git
url: https://github.com/tylerjw/serial.git
version: ros2
ros2_robotiq_gripper:
type: git
url: https://github.com/NikoFeith/ros2_robotiq_gripper.git
version: main
cartesian_controllers:
type: git
url: https://github.com/NikoFeith/cartesian_controllers.git

26
src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt
View File

@ -1,26 +0,0 @@
cmake_minimum_required(VERSION 3.8)
project(robotiq_2f_controllers)
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)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> 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()

18
src/robotiq_2f/robotiq_2f_controllers/package.xml
View File

@ -1,18 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>robotiq_2f_controllers</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

4
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro
View File

@ -19,7 +19,7 @@
</xacro:if>
<xacro:unless value="${use_fake_hardware}">
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
<param name="gripper_closed_position">0.14</param>
<param name="gripper_closed_position">0.695</param>
<param name="robot_ip">${robot_ip}</param>
<param name="robot_port">${robot_port}</param>
<param name="gripper_speed_multiplier">1.0</param>
@ -32,7 +32,7 @@
<joint name="${prefix}finger_joint">
<command_interface name="position" />
<state_interface name="position">
<param name="initial_value">0.14</param>
<param name="initial_value">0.695</param>
</state_interface>
<state_interface name="velocity"/>
</joint>

13
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.urdf.xacro
View File

@ -0,0 +1,13 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
<!-- parameters -->
<xacro:arg name="use_fake_hardware" default="false" />
<!-- Import macros -->
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
<link name="world" />
<xacro:robotiq_gripper name="RobotiqGripperHardwareInterface" prefix="" parent="world" use_fake_hardware="$(arg use_fake_hardware)">
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:robotiq_gripper>
</robot>

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

@ -0,0 +1,421 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
<xacro:macro name="robotiq_gripper" params="
name
prefix
parent
*origin
include_ros2_control:=true
use_fake_hardware:=false
fake_sensor_commands:=false
robot_ip:=192.168.1.1
robot_port:=63352>
<!-- ros2 control include -->
<xacro:include filename="$(find robotiq_2f_description)/urdf/2f_140.ros2_control.xacro" />
<!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
<xacro:if value="${include_ros2_control}">
<xacro:robotiq_gripper_ros2_control
name="${name}" prefix="${prefix}"
use_fake_hardware="${use_fake_hardware}"
fake_sensor_commands="${fake_sensor_commands}"
robot_ip="${robot_ip}"
robot_port="${robot_port}"/>
</xacro:if>
<!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->
<link name="${prefix}robotiq_base_link"/>
<joint name="${prefix}robotiq_base_joint" type="fixed">
<parent link="${parent}" />
<child link="${prefix}robotiq_base_link" />
<origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
</joint>
<link name="${prefix}robotiq_140_base_link">
<inertial>
<origin xyz="8.625E-08 -4.6583E-06 0.03145" rpy="0 0 0" />
<mass value="0.22652" />
<inertia ixx="0.00020005" ixy="-4.2442E-10" ixz="-2.9069E-10" iyy="0.00017832" iyz="-3.4402E-08" izz="0.00013478" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_base_link.stl" />
</geometry>
<material name="">
<color rgba="0.1 0.1 0.1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_base_link.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}robotiq_140_left_outer_knuckle">
<inertial>
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
<mass value="0.00853198276973456" />
<inertia
ixx="2.89328108496468E-06"
ixy="-1.57935047237397E-19"
ixz="-1.93980378593255E-19"
iyy="1.86719750325683E-06"
iyz="-1.21858577871576E-06"
izz="1.21905238907251E-06" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
</geometry>
<material name="">
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}robotiq_140_right_outer_knuckle">
<inertial>
<origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />
<mass value="0.00853198276973456" />
<inertia
ixx="2.89328108496468E-06"
ixy="-1.57935047237397E-19"
ixz="-1.93980378593255E-19"
iyy="1.86719750325683E-06"
iyz="-1.21858577871576E-06"
izz="1.21905238907251E-06" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_knuckle.stl" />
</geometry>
<material name="">
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_knuckle.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}robotiq_140_left_outer_finger">
<inertial>
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
<mass value="0.022614240507152" />
<inertia
ixx="1.52518312458174E-05"
ixy="9.76583423954399E-10"
ixz="-5.43838577022588E-10"
iyy="6.17694243867776E-06"
iyz="6.78636130740228E-06"
izz="1.16494917907219E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
</geometry>
<material name="">
<color rgba="0.1 0.1 0.1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}robotiq_140_right_outer_finger">
<inertial>
<origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />
<mass value="0.022614240507152" />
<inertia
ixx="1.52518312458174E-05"
ixy="9.76583423954399E-10"
ixz="-5.43838577022588E-10"
iyy="6.17694243867776E-06"
iyz="6.78636130740228E-06"
izz="1.16494917907219E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_outer_finger.stl" />
</geometry>
<material name="">
<color rgba="0.1 0.1 0.1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_outer_finger.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}robotiq_140_left_inner_knuckle">
<inertial>
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
<mass value="0.0271177346495152" />
<inertia
ixx="2.61910379223783E-05"
ixy="-2.43616858946494E-07"
ixz="-6.37789906117123E-09"
iyy="2.8270243746167E-06"
iyz="-5.37200748039765E-07"
izz="2.83695868220296E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
</geometry>
<material name="">
<color rgba="0.1 0.1 0.1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}robotiq_140_right_inner_knuckle">
<inertial>
<origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />
<mass value="0.0271177346495152" />
<inertia
ixx="2.61910379223783E-05"
ixy="-2.43616858946494E-07"
ixz="-6.37789906117123E-09"
iyy="2.8270243746167E-06"
iyz="-5.37200748039765E-07"
izz="2.83695868220296E-05" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_knuckle.stl" />
</geometry>
<material name="">
<color rgba="0.1 0.1 0.1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_knuckle.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}robotiq_140_left_inner_finger">
<inertial>
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
<mass value="0.0104003125914103" />
<inertia
ixx="2.71909453810972E-06"
ixy="1.35402465472579E-21"
ixz="-7.1817349065269E-22"
iyy="7.69100314106116E-07"
iyz="6.74715432769696E-07"
izz="2.30315190420171E-06" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
</geometry>
<material name="">
<color rgba="0.1 0.1 0.1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}robotiq_140_right_inner_finger">
<inertial>
<origin xyz="0.000299999999999317 0.0160078233491243 -0.0136945669206257" rpy="0 0 0" />
<mass value="0.0104003125914103" />
<inertia
ixx="2.71909453810972E-06"
ixy="1.35402465472579E-21"
ixz="-7.1817349065269E-22"
iyy="7.69100314106116E-07"
iyz="6.74715432769696E-07"
izz="2.30315190420171E-06" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_140/robotiq_2f_140_inner_finger.stl" />
</geometry>
<material name="">
<color rgba="0.1 0.1 0.1 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_140/robotiq_2f_140_inner_finger.stl" />
</geometry>
</collision>
</link>
<link name="${prefix}robotiq_140_left_inner_finger_pad">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.027 0.065 0.0075"/>
</geometry>
<material name="">
<color rgba="0.9 0.9 0.9 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.03 0.07 0.0075"/>
</geometry>
<material name="">
<color rgba="0.9 0.0 0.0 1" />
</material>
</collision>
</link>
<link name="${prefix}robotiq_140_right_inner_finger_pad">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.027 0.065 0.0075"/>
</geometry>
<material name="">
<color rgba="0.9 0.9 0.9 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.03 0.07 0.0075"/>
</geometry>
<material name="">
<color rgba="0.9 0.0 0.0 1" />
</material>
</collision>
</link>
<!-- Joints-->
<joint name="${prefix}robotiq_140_base_joint" type="fixed">
<parent link="${prefix}robotiq_base_link" />
<child link="${prefix}robotiq_140_base_link" />
<xacro:insert_block name="origin" />
</joint>
<joint name="${prefix}finger_joint" type="revolute">
<origin xyz="0 -0.030601 0.054905" rpy="${pi / 2 + .725} 0 0" />
<parent link="${prefix}robotiq_140_base_link" />
<child link="${prefix}robotiq_140_left_outer_knuckle" />
<axis xyz="-1 0 0" />
<limit lower="0" upper="0.7" velocity="2.0" effort="1000" />
</joint>
<joint name="${prefix}right_outer_knuckle_joint" type="revolute">
<origin xyz="0 0.030601 0.054905" rpy="${pi / 2 + .725} 0 ${pi}" />
<parent link="${prefix}robotiq_140_base_link" />
<child link="${prefix}robotiq_140_right_outer_knuckle" />
<axis xyz="1 0 0" />
<limit lower="-0.725" upper="0.725" velocity="2.0" effort="1000" />
<mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
</joint>
<joint name="${prefix}left_outer_finger_joint" type="fixed">
<origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
<parent link="${prefix}robotiq_140_left_outer_knuckle" />
<child link="${prefix}robotiq_140_left_outer_finger" />
<axis xyz="1 0 0" />
</joint>
<joint name="${prefix}right_outer_finger_joint" type="fixed">
<origin xyz="0 0.01821998610742 0.0260018192872234" rpy="0 0 0" />
<parent link="${prefix}robotiq_140_right_outer_knuckle" />
<child link="${prefix}robotiq_140_right_outer_finger" />
<axis xyz="1 0 0" />
</joint>
<joint name="${prefix}left_inner_knuckle_joint" type="revolute">
<origin xyz="0 -0.0127 0.06142" rpy="${pi / 2 + .725} 0 0" />
<parent link="${prefix}robotiq_140_base_link" />
<child link="${prefix}robotiq_140_left_inner_knuckle" />
<axis xyz="1 0 0" />
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
<mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
</joint>
<joint name="${prefix}right_inner_knuckle_joint" type="revolute">
<origin xyz="0 0.0127 0.06142" rpy="${pi / 2 + .725} 0 ${-pi}" />
<parent link="${prefix}robotiq_140_base_link" />
<child link="${prefix}robotiq_140_right_inner_knuckle" />
<axis xyz="1 0 0" />
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
<mimic joint="${prefix}finger_joint" multiplier="-1" offset="0" />
</joint>
<joint name="${prefix}left_inner_finger_joint" type="revolute">
<origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
<parent link="${prefix}robotiq_140_left_outer_finger" />
<child link="${prefix}robotiq_140_left_inner_finger" />
<axis xyz="1 0 0" />
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
<mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
</joint>
<joint name="${prefix}right_inner_finger_joint" type="revolute">
<origin xyz="0 0.0817554015893473 -0.0282203446692936" rpy="-0.725 0 0" />
<parent link="${prefix}robotiq_140_right_outer_finger" />
<child link="${prefix}robotiq_140_right_inner_finger" />
<axis xyz="1 0 0" />
<limit lower="-0.8757" upper="0.8757" velocity="2.0" effort="1000" />
<mimic joint="${prefix}finger_joint" multiplier="1" offset="0" />
</joint>
<joint name="${prefix}left_inner_finger_pad_joint" type="fixed">
<origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
<parent link="${prefix}robotiq_140_left_inner_finger" />
<child link="${prefix}robotiq_140_left_inner_finger_pad" />
<axis xyz="0 0 1" />
</joint>
<joint name="${prefix}right_inner_finger_pad_joint" type="fixed">
<origin xyz="0 0.0457554015893473 -0.0272203446692936" rpy="0 0 0" />
<parent link="${prefix}robotiq_140_right_inner_finger" />
<child link="${prefix}robotiq_140_right_inner_finger_pad" />
<axis xyz="0 0 1" />
</joint>
</xacro:macro>
</robot>

87
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro
View File

@ -0,0 +1,87 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="robotiq_gripper_ros2_control" params="
name
prefix
use_fake_hardware:=false
fake_sensor_commands:=false
robot_ip:=192.168.1.1
robot_port:=63352>
<ros2_control name="${name}" type="system">
<!-- Plugins -->
<hardware>
<xacro:if value="${use_fake_hardware}">
<plugin>mock_components/GenericSystem</plugin>
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
<param name="state_following_offset">0.0</param>
</xacro:if>
<xacro:unless value="${use_fake_hardware}">
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
<param name="gripper_closed_position">0.7929</param>
<param name="robot_ip">${robot_ip}</param>
<param name="robot_port">${robot_port}</param>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
</xacro:unless>
</hardware>
<!-- Joint interfaces -->
<!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
<joint name="${prefix}finger_joint">
<command_interface name="position" />
<state_interface name="position">
<param name="initial_value">0.7929</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<!-- When simulating we need to include the rest of the gripper joints -->
<xacro:if value="${use_fake_hardware}">
<joint name="${prefix}left_inner_knuckle_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}left_inner_finger_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}right_outer_knuckle_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}robotiq_140_left_finger_tip_joint">
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}robotiq_140_right_finger_tip_joint">
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</xacro:if>
<!-- Only add this with fake hardware mode -->
<gpio name="reactivate_gripper">
<command_interface name="reactivate_gripper_cmd" />
<command_interface name="reactivate_gripper_response" />
</gpio>
</ros2_control>
</xacro:macro>
</robot>

13
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.urdf.xacro
View File

@ -0,0 +1,13 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
<!-- parameters -->
<xacro:arg name="use_fake_hardware" default="true" />
<!-- Import macros -->
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
<link name="world" />
<xacro:robotiq_gripper name="RobotiqGripperHardwareInterface" prefix="" parent="world" use_fake_hardware="$(arg use_fake_hardware)">
<origin xyz="0 0 0" rpy="0 0 0" />
</xacro:robotiq_gripper>
</robot>

291
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro
View File

@ -0,0 +1,291 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
<xacro:macro name="robotiq_gripper" params="
name
prefix
parent
*origin
include_ros2_control:=true
use_fake_hardware:=false
fake_sensor_commands:=false
robot_ip:=192.168.1.1
robot_port:=63352>
<!-- ros2 control include -->
<xacro:include filename="$(find robotiq_2f_description)/urdf/2f_85.ros2_control.xacro" />
<!-- if we are simulating or directly communicating with the gripper we need a ros2 control instance -->
<xacro:if value="${include_ros2_control}">
<xacro:robotiq_gripper_ros2_control
name="${name}" prefix="${prefix}"
use_fake_hardware="${use_fake_hardware}"
fake_sensor_commands="${fake_sensor_commands}"
robot_ip="${robot_ip}"/>
robot_port="${robot_port}"/>
</xacro:if>
<link name="${prefix}robotiq_85_base_link">
<visual>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/robotiq_base.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/robotiq_base.stl" />
</geometry>
</collision>
<inertial>
<origin xyz="0.0 2.274e-05 0.03232288" rpy="0 0 0" />
<mass value="6.6320197e-01" />
<inertia ixx="5.1617816e-04" iyy="5.8802208e-04" izz="3.9462776e-04" ixy="2.936e-8" ixz="0.0" iyz="-3.2296e-7" />
</inertial>
</link>
<link name="${prefix}robotiq_85_left_knuckle_link">
<visual>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_knuckle.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_knuckle.stl" />
</geometry>
</collision>
<inertial>
<origin xyz="0.01213197 0.0002 -0.00058647" rpy="0 0 0" />
<mass value="1.384773208e-02" />
<inertia ixx="3.5232e-7" iyy="2.31944e-6" izz="2.23136e-6" ixy="0.0" ixz="1.1744e-7" iyz="0" />
</inertial>
</link>
<link name="${prefix}robotiq_85_right_knuckle_link">
<visual>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_knuckle.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_knuckle.stl" />
</geometry>
</collision>
<inertial>
<origin xyz="-0.01213197 0.0002 -0.00058647" rpy="0 0 0" />
<mass value="1.384773208e-02" />
<inertia ixx="3.5232e-7" iyy="2.31944e-6" izz="2.23136e-6" ixy="0.0" ixz="-1.1744e-7" iyz="0.0" />
</inertial>
</link>
<link name="${prefix}robotiq_85_left_finger_link">
<visual>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_finger.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_finger.stl" />
</geometry>
</collision>
<inertial>
<origin xyz="0.00346899 -0.00079447 0.01867121" rpy="0 0 0" />
<mass value="4.260376752e-02" />
<inertia ixx="1.385792000000000e-05" iyy="1.183208e-05" izz="5.19672e-06" ixy="0.0" ixz="-2.17264e-06" iyz="0.0" />
</inertial>
</link>
<link name="${prefix}robotiq_85_right_finger_link">
<visual>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_finger.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_finger.stl" />
</geometry>
</collision>
<inertial>
<origin xyz="-0.00346899 -5.53e-06 0.01867121" rpy="0 0 0" />
<mass value="4.260376752000000e-02" />
<inertia ixx="1.385792e-05" iyy="1.183208e-05" izz="5.19672e-06" ixy="0.0" ixz="2.17264e-06" iyz="0.0" />
</inertial>
</link>
<link name="${prefix}robotiq_85_left_inner_knuckle_link">
<visual>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_inner_knuckle.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_inner_knuckle.stl" />
</geometry>
</collision>
<inertial>
<origin xyz="0.01897699 0.00015001 0.02247101" rpy="0 0 0" />
<mass value="2.969376448e-02" />
<inertia ixx="9.57136e-06" iyy="8.69056e-06" izz="8.19144e-06" ixy="0.0" ixz="-3.93424e-06" iyz="0.0" />
</inertial>
</link>
<link name="${prefix}robotiq_85_right_inner_knuckle_link">
<visual>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_inner_knuckle.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_inner_knuckle.stl" />
</geometry>
</collision>
<inertial>
<origin xyz="-0.01926824 5.001e-05 0.02222178" rpy="0 0 0" />
<mass value="2.969376448e-02" />
<inertia ixx="9.42456e-06" iyy="8.69056e-06" izz="8.33824e-06" ixy="0.0" ixz="3.9636e-06" iyz="0.0" />
</inertial>
</link>
<link name="${prefix}robotiq_85_left_finger_tip_link">
<visual>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/left_finger_tip.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/left_finger_tip.stl" />
</geometry>
<surface>
<friction>
<ode>
<mu1>100000.0</mu1>
<mu2>100000.0</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>1e+5</kp>
<kd>1</kd>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<minDepth>0.002</minDepth>
<maxVel>0</maxVel>
</ode>
</contact>
</surface>
</collision>
<inertial>
<origin xyz="-0.01456706 -0.0008 0.01649701" rpy="0 0 0" />
<mass value="4.268588744e-02" />
<inertia ixx="1.048152e-05" iyy="1.197888e-05" izz="4.22784e-06" ixy="0.0" ixz="3.5232e-6" iyz="0.0" />
</inertial>
</link>
<link name="${prefix}robotiq_85_right_finger_tip_link">
<visual>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/visual/2f_85/right_finger_tip.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="file://$(find robotiq_2f_description)/meshes/collision/2f_85/right_finger_tip.stl" />
</geometry>
<surface>
<friction>
<ode>
<mu1>100000.0</mu1>
<mu2>100000.0</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>1e+5</kp>
<kd>1</kd>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<minDepth>0.002</minDepth>
<maxVel>0</maxVel>
</ode>
</contact>
</surface>
</collision>
<inertial>
<origin xyz="0.01456706 5e-05 0.01649701" rpy="0 0 0" />
<mass value="4.268588744e-02" />
<inertia ixx="1.048152e-05" iyy="1.197888e-05" izz="4.22784e-06" ixy="0.0" ixz="-3.5232e-06" iyz="0.0" />
</inertial>
</link>
<joint name="${prefix}robotiq_85_base_joint" type="fixed">
<parent link="${parent}" />
<child link="${prefix}robotiq_85_base_link" />
<xacro:insert_block name="origin" />
</joint>
<joint name="${prefix}robotiq_85_left_knuckle_joint" type="revolute">
<parent link="${prefix}robotiq_85_base_link" />
<child link="${prefix}robotiq_85_left_knuckle_link" />
<axis xyz="0 -1 0" />
<origin xyz="0.03060114 0.0 0.05490452" rpy="0 0 0" />
<limit lower="0.0" upper="0.8" velocity="0.5" effort="50" />
</joint>
<joint name="${prefix}robotiq_85_right_knuckle_joint" type="revolute">
<parent link="${prefix}robotiq_85_base_link" />
<child link="${prefix}robotiq_85_right_knuckle_link" />
<axis xyz="0 -1 0" />
<origin xyz="-0.03060114 0.0 0.05490452" rpy="0 0 0" />
<limit lower="-0.8" upper="0.0" velocity="0.5" effort="50" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
</joint>
<joint name="${prefix}robotiq_85_left_finger_joint" type="fixed">
<parent link="${prefix}robotiq_85_left_knuckle_link" />
<child link="${prefix}robotiq_85_left_finger_link" />
<origin xyz="0.03152616 0.0 -0.00376347" rpy="0 0 0" />
</joint>
<joint name="${prefix}robotiq_85_right_finger_joint" type="fixed">
<parent link="${prefix}robotiq_85_right_knuckle_link" />
<child link="${prefix}robotiq_85_right_finger_link" />
<origin xyz="-0.03152616 0.0 -0.00376347" rpy="0 0 0" />
</joint>
<joint name="${prefix}robotiq_85_left_inner_knuckle_joint" type="continuous">
<parent link="${prefix}robotiq_85_base_link" />
<child link="${prefix}robotiq_85_left_inner_knuckle_link" />
<axis xyz="0 -1 0" />
<origin xyz="0.0127 0.0 0.06142" rpy="0 0 0" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" />
</joint>
<joint name="${prefix}robotiq_85_right_inner_knuckle_joint" type="continuous">
<parent link="${prefix}robotiq_85_base_link" />
<child link="${prefix}robotiq_85_right_inner_knuckle_link" />
<axis xyz="0 -1 0" />
<origin xyz="-0.0127 0.0 0.06142" rpy="0 0 0" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
</joint>
<joint name="${prefix}robotiq_85_left_finger_tip_joint" type="continuous">
<parent link="${prefix}robotiq_85_left_finger_link" />
<child link="${prefix}robotiq_85_left_finger_tip_link" />
<axis xyz="0 -1 0" />
<origin xyz="0.00563134 0.0 0.04718515" rpy="0 0 0" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
</joint>
<joint name="${prefix}robotiq_85_right_finger_tip_joint" type="continuous">
<parent link="${prefix}robotiq_85_right_finger_link" />
<child link="${prefix}robotiq_85_right_finger_tip_link" />
<axis xyz="0 -1 0" />
<origin xyz="-0.00563134 0.0 0.04718515" rpy="0 0 0" />
<mimic joint="${prefix}robotiq_85_left_knuckle_joint" />
</joint>
</xacro:macro>
</robot>

54
src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt
View File

@ -1,28 +1,48 @@
cmake_minimum_required(VERSION 3.8)
cmake_minimum_required(VERSION 3.5)
project(robotiq_2f_interface)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
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()
include_directories(include)
# Declare a C++ library
add_library(${PROJECT_NAME} SHARED
src/hardware_interface.cpp
src/Robotiq2fSocketAdapter.cpp
)
ament_target_dependencies(${PROJECT_NAME}
rclcpp
hardware_interface
pluginlib
)
pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml)
# Install targets
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
# Install header files
install(DIRECTORY include/
DESTINATION include
)
# Install plugin description file
install(FILES
hardware_interface_plugin.xml
DESTINATION share/${PROJECT_NAME}
)
ament_package()

9
src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml Normal file
View File

@ -0,0 +1,9 @@
<library path="robotiq_driver_plugin">
<class name="robotiq_2f_interface/RobotiqGripperHardwareInterface"
type="robtiq_2f_interface::RobotiqGripperHardwareInterface"
base_class_type="hardware_interface::SystemInterface">
<description>
ROS2 controller for the Robotiq gripper.
</description>
</class>
</library>

101
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp Normal file
View File

@ -0,0 +1,101 @@
// MockRobotiq2fSocketAdapter.hpp
#ifndef MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
#define MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP
#include <string>
#include <map>
#include <atomic>
#include <tuple>
namespace robotiq{
class MockRobotiq2fSocketAdapter : public SocketAdapterBase {
public:
MockRobotiq2fSocketAdapter() : isConnected(false), gripperPosition(0), gripperForce(0), gripperSpeed(0), isActive(false) {}
~MockRobotiq2fSocketAdapter() override {
if (movementThread && movementThread->joinable()) {
movementThread->join();
}
}
bool connect(const std::string& hostname, int port) override {
isConnected.store(true);
return true; // Simulate successful connection
}
void disconnect() override {
isConnected.store(false);
}
int getGripperPosition() override {
return gripperPosition.load();
}
int getGripperForce() override {
// Assuming this is supposed to mimic `force()`
return gripperForce.load();
}
int force() override {
return gripperForce.load();
}
int speed() override {
return gripperSpeed.load();
}
int position() override {
return gripperPosition.load();
}
bool is_active() override {
return isActive.load();
}
void activate(bool autoCalibrate = true) override {
isActive.store(true);
}
void deactivate() override {
isActive.store(false);
}
std::tuple<bool, int> move(int targetPosition, int speed, int force) override{
// Ensure only one movement operation is active at a time
if (movementThread && movementThread->joinable()) {
movementThread->join(); // Wait for any ongoing movement to complete
}
// Start a new movement operation in a background thread
movementThread = std::make_unique<std::thread>(&MockRobotiq2fSocketAdapter::simulateMovement, this, targetPosition, speed, force);
return std::make_tuple(true, targetPosition); // Immediately return success and the target position
}
private:
std::atomic<bool> isConnected;
std::atomic<int> gripperPosition;
std::atomic<int> gripperForce;
std::atomic<int> gripperSpeed;
std::atomic<bool> isActive;
std::unique_ptr<std::thread> movementThread;
void simulateMovement(int targetPosition, int speed, int force) {
int step = (targetPosition > gripperPosition) ? 1 : -1;
// Simulate moving towards the target position
while (gripperPosition != targetPosition) {
gripperPosition += step;
std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Simulate time to move a step
}
// Here, you simulate setting the speed and force, though for simplicity, we're just logging it
// In a real mock, you might update some internal state to reflect these changes
std::cout << "Simulated movement to position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
}
};
} // end namespace
#endif // MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP

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

@ -25,6 +25,7 @@
#include <cerrno> // For errno
#include <iostream>
namespace robotiq{
// Enum for Gripper Status
enum class GripperStatus {
RESET = 0,
@ -41,11 +42,11 @@ enum class ObjectStatus {
AT_DEST = 3
};
class Robotiq2fSocketAdapter {
class Robotiq2fSocketAdapter : public SocketAdapterBase {
public:
Robotiq2fSocketAdapter() ;
~Robotiq2fSocketAdapter();
~Robotiq2fSocketAdapter() override;
// Connection management
/**
@ -60,7 +61,7 @@ public:
*
* @throws std::runtime_error if socket creation fails.
*/
bool connect(const std::string& hostname, int port);
bool connect(const std::string& hostname, int port) override;
/**
* Closes the connection to the Robotiq gripper.
*
@ -68,7 +69,7 @@ public:
* 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();
void disconnect() override;
/**
@ -119,12 +120,9 @@ public:
int getGripperVariable(const std::string& variable);
// Gripper status
bool isGripperActive();
int getGripperPosition();
int getGripperForce();
int force();
int speed();
int position();
int force() override;
int speed() override;
int position() override;
/**
* Checks if the gripper is currently active.
@ -137,7 +135,7 @@ public:
*
* @throws std::runtime_error if unable to retrieve the gripper's status, typically due to communication issues.
*/
bool is_active();
bool is_active() override;
// Activates the gripper and optionally performs auto-calibration
@ -158,7 +156,7 @@ public:
* activation command fails, or if the gripper does not reach the expected state within
* a reasonable timeframe.
*/
void activate(bool autoCalibrate = true);
void activate(bool autoCalibrate = true) override;
/**
* Performs auto-calibration of the gripper.
*
@ -175,7 +173,7 @@ public:
* @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);
void auto_calibrate(bool log=true) override;
/**
* Deactivates the gripper.
@ -188,7 +186,7 @@ public:
*
* @throws std::runtime_error if the adapter is not connected to the gripper or if the deactivation command fails.
*/
void deactivate();
void deactivate() override;
// Move Commands
/**
@ -204,7 +202,7 @@ public:
*
* @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<bool, int> move(int position, int speed, int force);
std::tuple<bool, int> move(int position, int speed, int force) override;
/**
* Commands the gripper to move to a specified position and waits for the move to complete.
*
@ -236,19 +234,19 @@ private:
};
// Smart pointer to manage the socket descriptor with the custom deleter
std::unique_ptr<int, SocketDeleter> _sockfd;
std::mutex socketMutex; // Mutex for socket access synchronization
std::unique_ptr<int, SocketDeleter> 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 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;
int open_position_ = min_position_;
int closed_position_ = max_position_;
/**
@ -352,5 +350,6 @@ private:
// Constants buffer sizes
const size_t BUFFER_SIZE = 1024; // Adjust as necessary
};
} // end namespace
#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP

25
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp Normal file
View File

@ -0,0 +1,25 @@
// IGripperAdapter.hpp
#ifndef SOCKET_ADAPTER_BASE_HPP
#define SOCKET_ADAPTER_BASE_HPP
#include <string>
#include <tuple>
namespace robotiq{
class SocketAdapterBase {
public:
virtual ~SocketAdapterBase() = default;
virtual bool connect(const std::string& hostname, int port) = 0;
virtual void disconnect() = 0;
virtual int force() = 0;
virtual int speed() = 0;
virtual int position() = 0;
virtual bool is_active() = 0;
virtual void activate(bool autoCalibrate = true) = 0;
virtual void deactivate() = 0;
virtual std::tuple<bool, int> move(int position, int speed, int force) = 0;
};
} // end namespace
#endif // SOCKET_ADAPTER_BASE_HPP

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

@ -10,18 +10,18 @@
#include <vector>
#include <atomic>
#include <Robotiq2fSocketAdapter.hpp>
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
#include "robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp"
#include <hardware_interface/actuator_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <rclcpp/rclcpp.hpp>
#include <urdf/model.h>
#include <stdexcept>
namespace robotiq_driver
{
namespace robotiq{
class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
{
public:
@ -46,8 +46,9 @@ public:
protected: // Likely changes the access to protected from private
// Members for driver interaction
std::unique_ptr<Robotiq2fSocketAdapter> socket_adapter_;
// std::unique_ptr<DriverFactory> driver_factory_;
bool use_mock_hardware_;
std::unique_ptr<SocketAdapterBase> socket_adapter_;
void RobotiqGripperHardwareInterface::configureAdapter(bool useMock);
// Background communication thread
std::thread communication_thread_;
@ -74,6 +75,8 @@ protected: // Likely changes the access to protected from private
double gripper_velocity_ = 0.0;
double gripper_position_command_ = 0.0;
rclcpp::Time last_update_time_;
double reactivate_gripper_cmd_ = 0.0;
std::atomic<bool> reactivate_gripper_async_cmd_;
double reactivate_gripper_response_ = 0.0;
@ -99,6 +102,6 @@ protected: // Likely changes the access to protected from private
// Logger
const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
};
}
} // end namespace
#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP

1
src/robotiq_2f/robotiq_2f_interface/package.xml
View File

@ -12,7 +12,6 @@
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>hardware_interface</depend>
<depend>lifecycle_msgs</depend>
<depend>pluginlib</depend>
<test_depend>ament_lint_auto</test_depend>

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

@ -1,7 +1,8 @@
#include "Robotiq2fSocketAdapter.hpp"
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : _sockfd(new int(-1)) {
namespace robotiq{
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
}
@ -11,8 +12,8 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
// Connection and disconnection
bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
std::lock_guard<std::mutex> lock(socketMutex);
if (*_sockfd >= 0) {
std::lock_guard<std::mutex> lock(socketMutex_);
if (*sockfd_ >= 0) {
std::cerr << "Already connected. Disconnecting to reconnect.\n";
disconnect();
}
@ -22,7 +23,7 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
return false;
}
*_sockfd = sock;
*sockfd_ = sock;
sockaddr_in serv_addr{};
memset(&serv_addr, 0, sizeof(serv_addr));
@ -31,15 +32,15 @@ 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";
close(*_sockfd);
*_sockfd = -1;
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";
close(*_sockfd);
*_sockfd = -1;
close(*sockfd_);
*sockfd_ = -1;
return false;
}
@ -47,26 +48,26 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
}
void Robotiq2fSocketAdapter::disconnect() {
std::lock_guard<std::mutex> lock(socketMutex);
if (*_sockfd >= 0) {
if (close(*_sockfd) == -1) {
std::lock_guard<std::mutex> 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;
*sockfd_ = -1;
}
}
// Utility methods
bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
std::lock_guard<std::mutex> lock(socketMutex);
if (*_sockfd < 0) {
std::lock_guard<std::mutex> 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);
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;
@ -76,8 +77,8 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
}
std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) {
std::lock_guard<std::mutex> lock(socketMutex);
if (*_sockfd < 0) {
std::lock_guard<std::mutex> lock(socketMutex_);
if (*sockfd_ < 0) {
std::cerr << "Attempted to receive response on a disconnected socket.\n";
return "";
}
@ -94,9 +95,9 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) {
while (!complete_response) {
fd_set readfds;
FD_ZERO(&readfds);
FD_SET(*_sockfd, &readfds);
FD_SET(*sockfd_, &readfds);
int activity = 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";
return "";
@ -105,7 +106,7 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) {
return "";
}
ssize_t bytes_read = recv(*_sockfd, buffer, BUFFER_SIZE - 1, 0);
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";
@ -166,8 +167,8 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
cmd += "\n";
// Send the command and receive the response
std::lock_guard<std::mutex> lock(socketMutex);
if (*_sockfd < 0) {
std::lock_guard<std::mutex> lock(socketMutex_);
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot set variables on a disconnected socket.");
return false;
}
@ -191,8 +192,8 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n";
std::lock_guard<std::mutex> lock(socketMutex);
if (*_sockfd < 0) {
std::lock_guard<std::mutex> lock(socketMutex_);
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot set variables on a disconnected socket.");
return false;
}
@ -209,8 +210,8 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n";
std::lock_guard<std::mutex> lock(socketMutex);
if (*_sockfd < 0) {
std::lock_guard<std::mutex> lock(socketMutex_);
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot get variables on a disconnected socket.");
return -1;
}
@ -288,9 +289,9 @@ 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);
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<std::string, int> variableMap = {
@ -305,9 +306,9 @@ 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);
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<std::string, int> variableMap = {
@ -353,17 +354,18 @@ void Robotiq2fSocketAdapter::auto_calibrate(bool log=true) {
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
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
min_position_ = std::get<0>(result); // Update min position
if (log) {
std::cout << "Gripper auto-calibrated to ["
<< _min_position << ", " << _max_position << "]\n";
<< min_position_ << ", " << max_position_ << "]\n";
}
}
} // end namespace

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

@ -1,11 +1,10 @@
#include "RobotiqGripperHardwareInterface.hpp"
#include "robotiq_2f_interface/hardware_interface.hpp"
namespace robotiq_driver
namespace robotiq
{
RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
: communication_thread_is_running_(false)
{
// socket_factory_ = std::make_unique<DefaultSocketFactory>();
}
RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
@ -41,6 +40,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
{
robot_ip_ = info_.hardware_parameters.at("robot_ip");
robot_port_ = std::stod(info_.hardware_parameters.at("robot_port"));
use_mock_hardware_ = std::stod(info_.hardware_parameters.at("use_fake_hardware"));
node->get_parameter("min_position", min_position_);
node->get_parameter("max_position", max_position_);
node->get_parameter("min_speed", min_speed_);
@ -98,8 +98,6 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
}
}
// Socket Factory -> not added yet
return CallbackReturn::SUCCESS;
}
@ -249,7 +247,7 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc
// Fetch current position and other states from the hardware
try {
int raw_position = socket_adapter_->getGripperPosition(); // This should be an existing method in your adapter
int raw_position = socket_adapter_->position(); // This should be an existing method in your adapter
double new_position = convertRawToPosition(raw_position);
rclcpp::Time current_time = time;
@ -309,6 +307,7 @@ void RobotiqGripperHardwareInterface::background_task()
{
while (communication_thread_is_running_.load())
{
std::lock_guard<std::mutex> guard(resource_mutex);
try
{
// Re-activate the gripper
@ -331,10 +330,10 @@ void RobotiqGripperHardwareInterface::background_task()
}
// Read the state of the gripper.
int raw_position = socket_adapter_->getGripperPosition();
int raw_position = socket_adapter_->position();
gripper_current_state_.store(convertRawToPosition(raw_position));
}
catch (std::exception& e)
catch (const std::exception& e)
{
RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
}
@ -344,6 +343,17 @@ void RobotiqGripperHardwareInterface::background_task()
}
// Mock Hardware
void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) {
if (useMock) {
RCLCPP_INFO(logger_, "Using mock Robotiq adapter");
socket_adapter_ = std::make_unique<MockRobotiq2fSocketAdapter>();
} else {
RCLCPP_INFO(logger_, "Using real Robotiq adapter");
socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
}
}
// Conversion methods
double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {

9
src/robotiq_2f/robotiq_controllers/CHANGELOG.rst Normal file
View File

@ -0,0 +1,9 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package robotiq_controllers
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.0.1 (2023-07-17)
------------------
* Initial ROS 2 release of robotiq_controllers
* This package is not supported by Robotiq but is being maintained by PickNik Robotics
* Contributors: Alex Moriarty, Cory Crean

84
src/robotiq_2f/robotiq_controllers/CMakeLists.txt Normal file
View File

@ -0,0 +1,84 @@
cmake_minimum_required(VERSION 3.8)
project(robotiq_controllers)
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(controller_interface REQUIRED)
find_package(std_srvs REQUIRED)
set(THIS_PACKAGE_INCLUDE_DEPENDS
controller_interface
std_srvs
)
include_directories(include)
add_library(${PROJECT_NAME} SHARED
src/robotiq_activation_controller.cpp
)
target_include_directories(${PROJECT_NAME} PRIVATE
include
)
ament_target_dependencies(${PROJECT_NAME}
${THIS_PACKAGE_INCLUDE_DEPENDS}
)
pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
# # INSTALL
install(
TARGETS ${PROJECT_NAME}
EXPORT export_${PROJECT_NAME}
ARCHIVE DESTINATION lib/${PROJECT_NAME}
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(
DIRECTORY include/
DESTINATION include
)
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)
# the following skips uncrustify
# ament_uncrustify and ament_clang_format cannot both be satisfied
set(ament_cmake_uncrustify_FOUND TRUE)
# the following skips xmllint
# ament_xmllint requires network and can timeout if on throttled networks
set(ament_cmake_xmllint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
# # EXPORTS
ament_export_include_directories(
include
)
ament_export_libraries(
${PROJECT_NAME}
)
ament_export_targets(
export_${PROJECT_NAME}
)
ament_export_dependencies(
${THIS_PACKAGE_INCLUDE_DEPENDS}
)
ament_package()

7
src/robotiq_2f/robotiq_controllers/controller_plugins.xml Normal file
View File

@ -0,0 +1,7 @@
<library path="robotiq_controllers">
<class name="robotiq_controllers/RobotiqActivationController" type="robotiq_controllers::RobotiqActivationController" base_class_type="controller_interface::ControllerInterface">
<description>
This controller provides an interface to (re-)activate the Robotiq gripper.
</description>
</class>
</library>

64
src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp Normal file
View File

@ -0,0 +1,64 @@
// Copyright (c) 2022 PickNik, Inc.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// * Neither the name of the {copyright_holder} nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include "controller_interface/controller_interface.hpp"
#include "std_srvs/srv/trigger.hpp"
namespace robotiq_controllers
{
class RobotiqActivationController : public controller_interface::ControllerInterface
{
public:
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
CallbackReturn on_init() override;
private:
bool reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr req,
std_srvs::srv::Trigger::Response::SharedPtr resp);
static constexpr double ASYNC_WAITING = 2.0;
enum CommandInterfaces
{
REACTIVATE_GRIPPER_CMD,
REACTIVATE_GRIPPER_RESPONSE
};
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reactivate_gripper_srv_;
};
} // namespace robotiq_controllers

23
src/robotiq_2f/robotiq_controllers/package.xml Normal file
View File

@ -0,0 +1,23 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>robotiq_controllers</name>
<version>0.0.1</version>
<description>Controllers for the Robotiq gripper.</description>
<maintainer email="alex.moriarty@picknik.ai">Alex Moriarty</maintainer>
<maintainer email="marq.rasmussen@picknik.ai">Marq Rasmussen</maintainer>
<license>BSD 3-Clause</license>
<author>Cory Crean</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>controller_interface</depend>
<depend>std_srvs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

123
src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp Normal file
View File

@ -0,0 +1,123 @@
// Copyright (c) 2022 PickNik, Inc.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// * Neither the name of the {copyright_holder} nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#include "robotiq_controllers/robotiq_activation_controller.hpp"
namespace robotiq_controllers
{
controller_interface::InterfaceConfiguration RobotiqActivationController::command_interface_configuration() const
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
config.names.emplace_back("reactivate_gripper/reactivate_gripper_cmd");
config.names.emplace_back("reactivate_gripper/reactivate_gripper_response");
return config;
}
controller_interface::InterfaceConfiguration RobotiqActivationController::state_interface_configuration() const
{
controller_interface::InterfaceConfiguration config;
config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
return config;
}
controller_interface::return_type RobotiqActivationController::update(const rclcpp::Time& /*time*/,
const rclcpp::Duration& /*period*/)
{
return controller_interface::return_type::OK;
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
RobotiqActivationController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
{
// Check command interfaces.
if (command_interfaces_.size() != 2)
{
RCLCPP_ERROR(get_node()->get_logger(), "Expected %d command interfaces, but got %zu.", 2,
command_interfaces_.size());
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
}
try
{
// Create service for re-activating the gripper.
reactivate_gripper_srv_ = get_node()->create_service<std_srvs::srv::Trigger>(
"~/reactivate_gripper",
[this](std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp) {
this->reactivateGripper(req, resp);
});
}
catch (...)
{
return LifecycleNodeInterface::CallbackReturn::ERROR;
}
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
RobotiqActivationController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
{
try
{
reactivate_gripper_srv_.reset();
}
catch (...)
{
return LifecycleNodeInterface::CallbackReturn::ERROR;
}
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqActivationController::on_init()
{
return LifecycleNodeInterface::CallbackReturn::SUCCESS;
}
bool RobotiqActivationController::reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
std_srvs::srv::Trigger::Response::SharedPtr resp)
{
command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING);
command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0);
while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING)
{
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value();
return resp->success;
}
} // namespace robotiq_controllers
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(robotiq_controllers::RobotiqActivationController, controller_interface::ControllerInterface)