From 02470cb0f3a70cf8abaf1cfd616a8cb8baf74a69 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 8 May 2024 09:30:48 +0200 Subject: [PATCH] Added rootiq Signed-off-by: Jakub Delicat --- config/robotiq_controllers.yaml | 13 +++++ launch/gz_components.launch.py | 4 ++ launch/gz_robotiq.launch.py | 94 +++++++++++++++++++++++++++++++++ package.xml | 5 ++ urdf/components.urdf.xacro | 27 ++++++++++ 5 files changed, 143 insertions(+) create mode 100644 config/robotiq_controllers.yaml create mode 100644 launch/gz_robotiq.launch.py diff --git a/config/robotiq_controllers.yaml b/config/robotiq_controllers.yaml new file mode 100644 index 0000000..6035346 --- /dev/null +++ b/config/robotiq_controllers.yaml @@ -0,0 +1,13 @@ +/**: + robotiq_gripper_controller: + ros__parameters: + default: true + joint: robotiq_85_left_knuckle_joint + use_effort_interface: true + use_speed_interface: true + + robotiq_activation_controller: + ros__parameters: + default: true + joint: robotiq_85_left_knuckle_joint + diff --git a/launch/gz_components.launch.py b/launch/gz_components.launch.py index 2a36791..93e36b2 100644 --- a/launch/gz_components.launch.py +++ b/launch/gz_components.launch.py @@ -98,6 +98,10 @@ def get_launch_descriptions_from_yaml_node( ) ) + if component["type"] == "GRP02": + actions.append(get_launch_description("robotiq", package, namespace, component)) + + return actions diff --git a/launch/gz_robotiq.launch.py b/launch/gz_robotiq.launch.py new file mode 100644 index 0000000..9fe19a6 --- /dev/null +++ b/launch/gz_robotiq.launch.py @@ -0,0 +1,94 @@ +# Copyright 2024 Husarion sp. z o.o. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.conditions import IfCondition, UnlessCondition, LaunchConfigurationNotEquals +from launch.actions import DeclareLaunchArgument, LogInfo +from launch_ros.actions import Node +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ReplaceString + +def generate_launch_description(): + robot_namespace = LaunchConfiguration("robot_namespace") + device_namespace = LaunchConfiguration("device_namespace") + tf_prefix = LaunchConfiguration("tf_prefix") + + initial_joint_controllers = PathJoinSubstitution( + [FindPackageShare("ros_components_description"), "config", "robotiq_controllers.yaml"] + ) + + namespace_warn = LogInfo( + msg="Namespace is not implemented with manipulators. Look here: https://github.com/ros-controls/ros2_control/issues/1506", + condition= LaunchConfigurationNotEquals(robot_namespace, "None") + ) + + # Using tf as namespace is caused by + # https://github.com/ros-controls/ros2_control/issues/1506 + # After this fix the device_namespace should be used. + namespaced_initial_joint_controllers_path = ReplaceString( + source_file=initial_joint_controllers, + replacements={ + "robotiq_85_left_knuckle_joint": [tf_prefix, "robotiq_85_left_knuckle_joint"], + " robotiq_gripper_controller:": [" ", tf_prefix, "robotiq_gripper_controller:"], + " robotiq_activation_controller:": [" ", tf_prefix, "robotiq_activation_controller:"], + }, + ) + + declare_device_namespace = DeclareLaunchArgument( + "device_namespace", + default_value="", + description="Sensor namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.", + ) + + declare_robot_namespace = DeclareLaunchArgument( + "robot_namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Namespace which will appear in front of all topics (including /tf and /tf_static).", + ) + + declare_tf_prefix = DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="Prefix added for all links of device. Here used as fix for static transform publisher.", + ) + + robotiq_gripper_controller = Node( + package="controller_manager", + executable="spawner", + arguments=[ + [tf_prefix, "robotiq_gripper_controller"], + "-t", + "position_controllers/GripperActionController", + "-c", + "controller_manager", + "--controller-manager-timeout", + "10", + "--namespace", + device_namespace, + "--param-file", + namespaced_initial_joint_controllers_path, + ], + namespace=robot_namespace, + ) + + return LaunchDescription( + [ + declare_device_namespace, + declare_robot_namespace, + declare_tf_prefix, + namespace_warn, + robotiq_gripper_controller, + ] + ) diff --git a/package.xml b/package.xml index c82c31a..18c8671 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,7 @@ xacro ur_description kortex_description + robotiq_description gazebo_plugins @@ -43,6 +44,10 @@ nav2_common + + robotiq_controllers + + ament_index_python ament_python_pytest python-yaml diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro index b8f5541..107babf 100644 --- a/urdf/components.urdf.xacro +++ b/urdf/components.urdf.xacro @@ -236,6 +236,33 @@ /> + + + + + + + + + + + + + + +