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

Mock system running

Browse Source 
Controllers not tested
This commit is contained in:
Niko Feith 2024-04-12 17:07:41 +02:00
parent a59a248969
commit d2de869e86
19 changed files with 738 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_85_config.yaml
View File

@ -1,7 +1,4 @@
robotiq_2f_140_gripper:
ros__parameters:
robot_ip: "192.168.1.11"
robot_port: 63352
robotiq_2f_gripper:
# Physical limits
min_position: 0.0 # Meters (fully closed)
max_position: 0.085 # Meters (fully open)

20
src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml
View File

@ -0,0 +1,20 @@
controller_manager:
ros__parameters:
update_rate: 500 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
robotiq_gripper_controller:
type: position_controllers/GripperActionController
robotiq_activation_controller:
type: robotiq_controllers/RobotiqActivationController
robotiq_gripper_controller:
ros__parameters:
default: true
joint: finger_joint
use_effort_interface: true
use_speed_interface: true
robotiq_activation_controller:
ros__parameters:
default: true

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

@ -0,0 +1,148 @@
# 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.
import launch
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
from launch.conditions import IfCondition
import launch_ros
import os
def generate_launch_description():
description_pkg_share = launch_ros.substitutions.FindPackageShare(
package="robotiq_description"
).find("robotiq_2f_description")
default_model_path = os.path.join(
description_pkg_share, "urdf", "robotiq_2f_140.urdf.xacro"
)
default_rviz_config_path = os.path.join(
description_pkg_share, "rviz", "view_urdf.rviz"
)
args = []
args.append(
launch.actions.DeclareLaunchArgument(
name="model",
default_value=default_model_path,
description="Absolute path to gripper URDF file",
)
)
args.append(
launch.actions.DeclareLaunchArgument(
name="rvizconfig",
default_value=default_rviz_config_path,
description="Absolute path to rviz config file",
)
)
args.append(
launch.actions.DeclareLaunchArgument(
name="launch_rviz", default_value="true", description="Launch RViz?"
)
)
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
LaunchConfiguration("model"),
" ",
"use_fake_hardware:=true",
]
)
robot_description_param = {
"robot_description": launch_ros.parameter_descriptions.ParameterValue(
robot_description_content, value_type=str
)
}
controllers_file = "robotiq_controllers.yaml"
initial_joint_controllers = PathJoinSubstitution(
[description_pkg_share, "config", controllers_file]
)
control_node = launch_ros.actions.Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[
robot_description_param,
initial_joint_controllers,
],
)
robot_state_publisher_node = launch_ros.actions.Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[robot_description_param],
)
rviz_node = launch_ros.actions.Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", LaunchConfiguration("rvizconfig")],
condition=IfCondition(LaunchConfiguration("launch_rviz")),
)
joint_state_broadcaster_spawner = launch_ros.actions.Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster",
"--controller-manager",
"/controller_manager",
],
)
robotiq_gripper_controller_spawner = launch_ros.actions.Node(
package="controller_manager",
executable="spawner",
arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
)
robotiq_activation_controller_spawner = launch_ros.actions.Node(
package="controller_manager",
executable="spawner",
arguments=["robotiq_activation_controller", "-c", "/controller_manager"],
)
nodes = [
control_node,
robot_state_publisher_node,
joint_state_broadcaster_spawner,
robotiq_gripper_controller_spawner,
robotiq_activation_controller_spawner,
rviz_node,
]
return launch.LaunchDescription(args + nodes)

75
src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py
View File

@ -0,0 +1,75 @@
import launch
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
PathJoinSubstitution,
)
import launch_ros
import os
def generate_launch_description():
pkg_share = launch_ros.substitutions.FindPackageShare(
package="robotiq_2f_description"
).find("robotiq_2f_description")
default_model_path = os.path.join(
pkg_share, "urdf", "robotiq_2f_85.urdf.xacro"
)
default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_urdf.rviz")
args = []
args.append(
launch.actions.DeclareLaunchArgument(
name="model",
default_value=default_model_path,
description="Absolute path to gripper URDF file",
)
)
args.append(
launch.actions.DeclareLaunchArgument(
name="rvizconfig",
default_value=default_rviz_config_path,
description="Absolute path to rviz config file",
)
)
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
LaunchConfiguration("model"),
]
)
robot_description_param = {
"robot_description": launch_ros.parameter_descriptions.ParameterValue(
robot_description_content, value_type=str
)
}
robot_state_publisher_node = launch_ros.actions.Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[robot_description_param],
)
joint_state_publisher_node = launch_ros.actions.Node(
package="joint_state_publisher_gui",
executable="joint_state_publisher_gui",
)
rviz_node = launch_ros.actions.Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="screen",
arguments=["-d", LaunchConfiguration("rvizconfig")],
)
nodes = [
robot_state_publisher_node,
joint_state_publisher_node,
rviz_node,
]
return launch.LaunchDescription(args + nodes)

234
src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz Normal file
View File

@ -0,0 +1,234 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Status1
Splitter Ratio: 0.6264705657958984
Tree Height: 555
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
dummy_link:
Alpha: 1
Show Axes: false
Show Trail: false
end_effector_link:
Alpha: 1
Show Axes: false
Show Trail: false
forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
gripper_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_finger_dist_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_finger_prox_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
lower_wrist_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_finger_dist_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_finger_prox_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
table:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
tool_frame:
Alpha: 1
Show Axes: false
Show Trail: false
upper_wrist_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
Mass Properties:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 2.1567115783691406
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.09681572020053864
Y: -0.10843408107757568
Z: 0.1451336145401001
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.785398006439209
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.785398006439209
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 1989
Y: 261

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

@ -4,27 +4,37 @@
<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>
use_fake_hardware
fake_sensor_commands
robot_ip
robot_port
min_position
max_position
min_speed
max_speed
min_force
max_force">
<ros2_control name="${name}" type="system">
<!-- Plugins -->
<hardware>
<xacro:if value="${use_fake_hardware}">
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
<xacro:if value="${use_fake_hardware}">
<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.695</param>
<param name="robot_ip">${robot_ip}</param>
<param name="robot_port">${robot_port}</param>
<param name="use_fake_hardware">${use_fake_hardware}</param>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
</xacro:unless>
<param name="min_position">${min_position}</param>
<param name="max_position">${max_position}</param>
<param name="min_speed">${min_speed}</param>
<param name="max_speed">${max_speed}</param>
<param name="min_force">${min_force}</param>
<param name="max_force">${max_force}</param>
</hardware>
<!-- Joint interfaces -->
@ -37,7 +47,7 @@
<state_interface name="velocity"/>
</joint>
<!-- When simulating we need to include the rest of the gripper joints -->
<xacro:if value="${use_fake_hardware}">
<xacro:if value="false">
<joint name="${prefix}left_inner_knuckle_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">-1</param>

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

@ -1,6 +1,5 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
<xacro:include filename="$(find robotiq_2f_description)/config/robotiq_2f_140_config.yaml" />
<xacro:macro name="robotiq_gripper" params="
name
prefix
@ -10,10 +9,20 @@
use_fake_hardware:=false
fake_sensor_commands:=false
robot_ip:=192.168.1.1
robot_port:=63352>
robot_port:=63352">
<xacro:property name="config_file" value="$(find robotiq_2f_description)/config/robotiq_2f_140_config.yaml" />
<xacro:property name="config_dict" value="${xacro.load_yaml(config_file)}" />
<xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
<xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
<xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
<xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
<xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
<xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
<xacro:property name="max_force" value="${robotiq_2f_gripper_dict['max_force']}" />
<!-- ros2 control include -->
<xacro:include filename="$(find robotiq_2f_description)/urdf/2f_140.ros2_control.xacro" />
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_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
@ -22,12 +31,12 @@
fake_sensor_commands="${fake_sensor_commands}"
robot_ip="${robot_ip}"
robot_port="${robot_port}"
min_position="${robotiq_2f_gripper.min_position}"
max_position="${robotiq_2f_gripper.max_position}"
min_speed="${robotiq_2f_gripper.min_speed}"
max_speed="${robotiq_2f_gripper.max_speed}"
min_force="${robotiq_2f_gripper.min_force}"
max_force="${robotiq_2f_gripper.max_force}"/>
min_position="${min_position}"
max_position="${max_position}"
min_speed="${min_speed}"
max_speed="${max_speed}"
min_force="${min_force}"
max_force="${max_force}"/>
</xacro:if>
<!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->

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

@ -4,10 +4,16 @@
<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>
use_fake_hardware
fake_sensor_commands
robot_ip
robot_port
min_position
max_position
min_speed
max_speed
min_force
max_force">
<ros2_control name="${name}" type="system">
<!-- Plugins -->
@ -22,8 +28,15 @@
<param name="gripper_closed_position">0.7929</param>
<param name="robot_ip">${robot_ip}</param>
<param name="robot_port">${robot_port}</param>
<param name="use_fake_hardware">${use_fake_hardware}</param>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="min_position">${min_position}</param>
<param name="max_position">${max_position}</param>
<param name="min_speed">${min_speed}</param>
<param name="max_speed">${max_speed}</param>
<param name="min_force">${min_force}</param>
<param name="max_force">${max_force}</param>
</xacro:unless>
</hardware>

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

@ -1,7 +1,7 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
<!-- parameters -->
<xacro:arg name="use_fake_hardware" default="true" />
<xacro:arg name="use_fake_hardware" default="false" />
<!-- Import macros -->
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />

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

@ -1,6 +1,5 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
<xacro:include filename="$(find robotiq_2f_description)/config/robotiq_2f_85_config.yaml" />
<xacro:macro name="robotiq_gripper" params="
name
prefix
@ -10,24 +9,34 @@
use_fake_hardware:=false
fake_sensor_commands:=false
robot_ip:=192.168.1.1
robot_port:=63352>
robot_port:=63352">
<xacro:property name="config_file" value="$(find robotiq_2f_description)/config/robotiq_2f_85_config.yaml" />
<xacro:property name="config_dict" value="${xacro.load_yaml(config_file)}" />
<xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
<xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
<xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
<xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
<xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
<xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
<xacro:property name="max_force" value="${robotiq_2f_gripper_dict['max_force']}" />
<!-- ros2 control include -->
<xacro:include filename="$(find robotiq_2f_description)/urdf/2f_85.ros2_control.xacro" />
<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_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_ip="${robot_ip}"
robot_port="${robot_port}"
min_position="${robotiq_2f_gripper.min_position}"
max_position="${robotiq_2f_gripper.max_position}"
min_speed="${robotiq_2f_gripper.min_speed}"
max_speed="${robotiq_2f_gripper.max_speed}"
min_force="${robotiq_2f_gripper.min_force}"
max_force="${robotiq_2f_gripper.max_force}"/>
min_position="${min_position}"
max_position="${max_position}"
min_speed="${min_speed}"
max_speed="${max_speed}"
min_force="${min_force}"
max_force="${max_force}"/>
</xacro:if>
<link name="${prefix}robotiq_85_base_link">

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

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.5)
cmake_minimum_required(VERSION 3.8)
project(robotiq_2f_interface)
# Default to C++17

4
src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml
View File

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

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

@ -10,7 +10,8 @@
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
namespace robotiq{
namespace robotiq_interface
{
class MockRobotiq2fSocketAdapter : public SocketAdapterBase {
public:
MockRobotiq2fSocketAdapter() : isConnected(false), gripperPosition(0), gripperForce(0), gripperSpeed(0), isActive(false) {}
@ -89,6 +90,6 @@ private:
std::cout << "Simulated movement to position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
}
};
} // end namespace
} // end robotiq_interface
#endif // MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP

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

@ -27,7 +27,8 @@
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
namespace robotiq{
namespace robotiq_interface
{
// Enum for Gripper Status
enum class GripperStatus {
RESET = 0,
@ -354,6 +355,6 @@ private:
const int MAX_RETRIES = 5;
const int RETRY_DELAY_MS = 20;
};
} // end namespace
} // end robotiq_interface
#endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP

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

@ -6,7 +6,8 @@
#include <string>
#include <tuple>
namespace robotiq{
namespace robotiq_interface
{
class SocketAdapterBase {
public:
virtual ~SocketAdapterBase() = default;
@ -21,5 +22,5 @@ public:
virtual void deactivate() = 0;
virtual std::tuple<bool, int> move(int position, int speed, int force) = 0;
};
} // end namespace
} // end robotiq_interface
#endif // SOCKET_ADAPTER_BASE_HPP

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

@ -21,32 +21,35 @@
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/clock.hpp>
#include <stdexcept>
namespace robotiq{
namespace robotiq_interface
{
class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface
{
public:
// Constructors
ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface();
ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface();
ROBOTIQ_INTERFACE_PUBLIC RobotiqGripperHardwareInterface();
ROBOTIQ_INTERFACE_PUBLIC ~RobotiqGripperHardwareInterface();
// ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr<SocketFactory> socket_factory);
// Lifecycle Management
ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override;
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
// State and Command Interfaces
ROBOTIQ_DRIVER_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
ROBOTIQ_DRIVER_PUBLIC std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
ROBOTIQ_INTERFACE_PUBLIC std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
ROBOTIQ_INTERFACE_PUBLIC std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
// Data Access (Read/Write)
ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
ROBOTIQ_INTERFACE_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
ROBOTIQ_INTERFACE_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
protected: // Likely changes the access to protected from private
// Members for driver interaction
@ -80,7 +83,9 @@ 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_;
rclcpp::Clock::SharedPtr clock_ = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
rclcpp::Time last_update_time_ = clock_ -> now();
rclcpp::Time current_time_ = clock_ -> now();
double reactivate_gripper_cmd_ = 0.0;
std::atomic<bool> reactivate_gripper_async_cmd_;
@ -107,6 +112,6 @@ protected: // Likely changes the access to protected from private
// Logger
const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
};
} // end namespace
} // end robotiq_interface
#endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP

36
src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp
View File

@ -19,38 +19,38 @@
* library cannot have, but the consuming code must have inorder to link.
*/
#ifndef ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_
#define ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_
#ifndef ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_
#define ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define ROBOTIQ_DRIVER_EXPORT __attribute__((dllexport))
#define ROBOTIQ_DRIVER_IMPORT __attribute__((dllimport))
#define ROBOTIQ_INTERFACE_EXPORT __attribute__((dllexport))
#define ROBOTIQ_INTERFACE_IMPORT __attribute__((dllimport))
#else
#define ROBOTIQ_DRIVER_EXPORT __declspec(dllexport)
#define ROBOTIQ_DRIVER_IMPORT __declspec(dllimport)
#define ROBOTIQ_INTERFACE_EXPORT __declspec(dllexport)
#define ROBOTIQ_INTERFACE_IMPORT __declspec(dllimport)
#endif
#ifdef ROBOTIQ_DRIVER_BUILDING_DLL
#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_EXPORT
#ifdef ROBOTIQ_INTERFACE_BUILDING_DLL
#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_EXPORT
#else
#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_IMPORT
#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_IMPORT
#endif
#define ROBOTIQ_DRIVER_PUBLIC_TYPE ROBOTIQ_DRIVER_PUBLIC
#define ROBOTIQ_DRIVER_LOCAL
#define ROBOTIQ_INTERFACE_PUBLIC_TYPE ROBOTIQ_INTERFACE_PUBLIC
#define ROBOTIQ_INTERFACE_LOCAL
#else
#define ROBOTIQ_DRIVER_EXPORT __attribute__((visibility("default")))
#define ROBOTIQ_DRIVER_IMPORT
#define ROBOTIQ_INTERFACE_EXPORT __attribute__((visibility("default")))
#define ROBOTIQ_INTERFACE_IMPORT
#if __GNUC__ >= 4
#define ROBOTIQ_DRIVER_PUBLIC __attribute__((visibility("default")))
#define ROBOTIQ_DRIVER_LOCAL __attribute__((visibility("hidden")))
#define ROBOTIQ_INTERFACE_PUBLIC __attribute__((visibility("default")))
#define ROBOTIQ_INTERFACE_LOCAL __attribute__((visibility("hidden")))
#else
#define ROBOTIQ_DRIVER_PUBLIC
#define ROBOTIQ_DRIVER_LOCAL
#define ROBOTIQ_INTERFACE_PUBLIC
#define ROBOTIQ_INTERFACE_LOCAL
#endif
#define ROBOTIQ_DRIVER_PUBLIC_TYPE
#define ROBOTIQ_INTERFACE_PUBLIC_TYPE
#endif
#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_

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

@ -1,7 +1,8 @@
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
namespace robotiq{
namespace robotiq_interface
{
Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) {
}
@ -368,4 +369,4 @@ void Robotiq2fSocketAdapter::auto_calibrate(bool log) {
<< min_position_ << ", " << max_position_ << "]\n";
}
}
} // end namespace
} // end robotiq_interface

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

@ -1,10 +1,39 @@
#include "robotiq_2f_interface/hardware_interface.hpp"
namespace robotiq
namespace robotiq_interface
{
const std::string robot_ip_parameter_name = "robot_ip";
const std::string robot_ip_default = "192.168.1.1";
const std::string robot_port_parameter_name = "robot_port";
const int robot_port_default = 63352;
const std::string use_mock_hardware_parameter_name = "use_fake_hardware";
const bool use_mock_hardware_default = true;
const std::string min_position_parameter_name = "min_position";
const double min_position_default = 0.01;
const std::string max_position_parameter_name = "max_position";
const double max_position_default = 0.0;
const std::string min_speed_parameter_name = "min_speed";
const double min_speed_default = 0.0;
const std::string max_speed_parameter_name = "max_speed";
const double max_speed_default = 0.0;
const std::string min_force_parameter_name = "min_force";
const double min_force_default = 0.0;
const std::string max_force_parameter_name = "max_force";
const double max_force_default = 0.0;
RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
: communication_thread_is_running_(false)
{
RCLCPP_INFO(logger_, "Constructor");
}
RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
@ -21,7 +50,7 @@ RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
{
RCLCPP_DEBUG(rclcpp::get_logger("RobotiqGripperHardwareInterface"), "on_init");
RCLCPP_DEBUG(logger_, "on_init");
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
{
@ -30,20 +59,74 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
try
{
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"));
min_position_ = std::stod(info_.hardware_parameters.at("min_position"));
max_position_ = std::stod(info_.hardware_parameters.at("max_position"));
min_speed_ = std::stod(info_.hardware_parameters.at("min_speed"));
max_speed_ = std::stod(info_.hardware_parameters.at("max_speed"));
min_force_ = std::stod(info_.hardware_parameters.at("min_force"));
max_force_ = std::stod(info_.hardware_parameters.at("max_force"));
if (info.hardware_parameters.count(robot_ip_parameter_name) > 0) {
robot_ip_ = info.hardware_parameters.at(robot_ip_parameter_name);
} else {
robot_ip_ = robot_ip_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: Robot IP, Value: %s", robot_ip_.c_str());
if (info.hardware_parameters.count(robot_port_parameter_name) > 0) {
robot_port_ = std::stoi(info.hardware_parameters.at(robot_port_parameter_name));
} else {
robot_port_ = robot_port_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: Robot Port, Value: %d", robot_port_);
std::string param_value;
if (info.hardware_parameters.count(use_mock_hardware_parameter_name) > 0) {
param_value = info.hardware_parameters.at(use_mock_hardware_parameter_name);
use_mock_hardware_ = (param_value == "true" || param_value == "True");
} else {
use_mock_hardware_ = use_mock_hardware_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: use fake hardware , Value: %s", param_value.c_str());
if (info.hardware_parameters.count(min_position_parameter_name) > 0) {
min_position_ = std::stod(info.hardware_parameters.at(min_position_parameter_name));
} else {
min_position_ = min_position_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: min position, Value: %f", min_position_);
if (info.hardware_parameters.count(max_position_parameter_name) > 0) {
max_position_ = std::stod(info.hardware_parameters.at(max_position_parameter_name));
} else {
max_position_ = max_position_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: max position, Value: %f", max_position_);
if (info.hardware_parameters.count(min_speed_parameter_name) > 0) {
min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name));
} else {
min_speed_ = min_speed_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: min speed, Value: %f", min_speed_);
if (info.hardware_parameters.count(max_speed_parameter_name) > 0) {
max_speed_ = std::stod(info.hardware_parameters.at(max_speed_parameter_name));
} else {
max_speed_ = max_speed_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: max speed, Value: %f", max_speed_);
if (info.hardware_parameters.count(min_force_parameter_name) > 0) {
min_force_ = std::stod(info.hardware_parameters.at(min_force_parameter_name));
} else {
min_force_ = robot_port_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: min force, Value: %f", min_force_);
if (info.hardware_parameters.count(max_force_parameter_name) > 0) {
max_force_ = std::stod(info.hardware_parameters.at(max_force_parameter_name));
} else {
max_force_ = max_force_default;
}
RCLCPP_INFO(logger_, "Accessing parameter: max force, Value: %f", max_force_);
}
catch (const std::exception& e)
{
RCLCPP_FATAL(rclcpp::get_logger("RobotiqGripperHardwareInterface"),
"Failed to load parameters: %s", e.what());
RCLCPP_FATAL(logger_, "Failed to load parameters: %s", e.what());
return CallbackReturn::ERROR;
}
@ -103,7 +186,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure
return CallbackReturn::ERROR;
}
socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
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_);
@ -181,16 +264,14 @@ std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface:
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back(
hardware_interface::StateInterface(
info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_
)
);
hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_));
state_interfaces.emplace_back(
hardware_interface::StateInterface(
info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_
)
);
hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_));
for (const auto& interface : state_interfaces) {
RCLCPP_DEBUG(logger_, "Exporting state interface for joint: %s type: %s",
interface.get_name().c_str(), interface.get_interface_name().c_str());
}
return state_interfaces;
}
@ -230,41 +311,41 @@ std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterfac
command_interfaces.emplace_back(
hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_));
for (const auto& interface : command_interfaces) {
RCLCPP_DEBUG(logger_, "Exporting command interface for joint: %s type: %s",
interface.get_name().c_str(), interface.get_interface_name().c_str());
}
return command_interfaces;
}
hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& time, const rclcpp::Duration& /*period*/)
hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
{
RCLCPP_DEBUG(logger_, "Reading state from the gripper");
// Fetch current position and other states from the hardware
// Ensure that the node uses the appropriate clock source
current_time_ = clock_->now();
try {
int raw_position = socket_adapter_->position(); // This should be an existing method in your adapter
int raw_position = socket_adapter_->position();
double new_position = convertRawToPosition(raw_position);
rclcpp::Time current_time = time;
if (!std::isnan(gripper_position_)) {
double time_difference = (current_time - last_update_time_).seconds(); // Calculate time difference in seconds
double time_difference = (current_time_ - last_update_time_).seconds();
if (time_difference > 0) {
gripper_velocity_ = (new_position - gripper_position_) / time_difference; // Calculate velocity
gripper_velocity_ = (new_position - gripper_position_) / time_difference;
}
}
gripper_position_ = new_position; // Update current position
last_update_time_ = current_time; // Update last update timestamp
gripper_position_ = new_position;
last_update_time_ = current_time_;
} catch (const std::exception& e) {
RCLCPP_ERROR(logger_, "Failed to read from Robotiq gripper: %s", e.what());
return hardware_interface::return_type::ERROR;
}
// Handle reactivation command if set
if (!std::isnan(reactivate_gripper_cmd_) && reactivate_gripper_cmd_ != NO_NEW_CMD_) {
RCLCPP_INFO(logger_, "Sending gripper reactivation request.");
reactivate_gripper_async_cmd_.store(true);
reactivate_gripper_cmd_ = NO_NEW_CMD_; // Reset command
}
// Process asynchronous reactivation response if available
if (reactivate_gripper_async_response_.load().has_value()) {
reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value();
@ -339,10 +420,10 @@ void RobotiqGripperHardwareInterface::background_task()
// Mock Hardware
void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) {
if (useMock) {
RCLCPP_INFO(logger_, "Using mock Robotiq adapter");
RCLCPP_INFO(logger_, "Using Mock Robotiq Adapter");
socket_adapter_ = std::make_unique<MockRobotiq2fSocketAdapter>();
} else {
RCLCPP_INFO(logger_, "Using real Robotiq adapter");
RCLCPP_INFO(logger_, "Using Real Robotiq Adapter");
socket_adapter_ = std::make_unique<Robotiq2fSocketAdapter>();
}
}
@ -381,8 +462,8 @@ int RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
}
} // namespace robotiq_driver
} // namespace robotiq_interface
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(robotiq::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)
PLUGINLIB_EXPORT_CLASS(robotiq_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)