Mock system running
Controllers not tested
This commit is contained in:
parent
a59a248969
commit
d2de869e86
@ -1,11 +1,8 @@
|
||||
robotiq_2f_140_gripper:
|
||||
ros__parameters:
|
||||
robot_ip: "192.168.1.11"
|
||||
robot_port: 63352
|
||||
# Physical limits
|
||||
min_position: 0.0 # Meters (fully closed)
|
||||
max_position: 0.085 # Meters (fully open)
|
||||
min_speed: 0.02 # Meters per second
|
||||
max_speed: 0.15 # Meters per second
|
||||
min_force: 20.0 # Newtons
|
||||
max_force: 235.0 # Newtons
|
||||
robotiq_2f_gripper:
|
||||
# Physical limits
|
||||
min_position: 0.0 # Meters (fully closed)
|
||||
max_position: 0.085 # Meters (fully open)
|
||||
min_speed: 0.02 # Meters per second
|
||||
max_speed: 0.15 # Meters per second
|
||||
min_force: 20.0 # Newtons
|
||||
max_force: 235.0 # Newtons
|
@ -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
|
@ -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)
|
@ -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
234
src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz
Normal 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
|
@ -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>
|
||||
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
|
||||
<xacro:if value="${use_fake_hardware}">
|
||||
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</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.695</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>
|
||||
<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>
|
||||
<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>
|
||||
|
@ -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 -->
|
||||
|
@ -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>
|
||||
|
||||
|
@ -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" />
|
||||
|
@ -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">
|
||||
|
@ -1,4 +1,4 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(robotiq_2f_interface)
|
||||
|
||||
# Default to C++17
|
||||
|
@ -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.
|
||||
|
@ -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
|
@ -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
|
||||
|
@ -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
|
@ -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
|
@ -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_
|
@ -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
|
@ -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)
|
||||
|
Loading…
Reference in New Issue
Block a user