From f50cee03b163120071e38d7b70f39befd9320a33 Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Thu, 23 May 2024 13:33:30 +0200 Subject: [PATCH] ROS 2 add UR, Kinova, Robotiq components (#44) * Fixed prefix Signed-off-by: Jakub Delicat * Added launches and gz mappings Signed-off-by: Jakub Delicat * update launches Signed-off-by: Jakub Delicat * fixed astra Signed-off-by: Jakub Delicat * removed print Signed-off-by: Jakub Delicat * Fixed unused file Signed-off-by: Jakub Delicat * renamed files Signed-off-by: Jakub Delicat * Added ur Signed-off-by: Jakub Delicat * added kinova Signed-off-by: Jakub Delicat * split 6dof and 7dof Signed-off-by: Jakub Delicat * Added rootiq Signed-off-by: Jakub Delicat * Added suggestion Signed-off-by: Jakub Delicat * simplfied urfd | removed use_gpu | removed simulation-engine Signed-off-by: Jakub Delicat * simplfied urfd | removed use_gpu | removed simulation-engine Signed-off-by: Jakub Delicat * applied new changes to ur Signed-off-by: Jakub Delicat * removed simulation-engine from s3 | fix with no namespace Signed-off-by: Jakub Delicat * fixed namespaced ros2_control Signed-off-by: Jakub Delicat * Suggestions * add * workflow simplify * Removed unnecessary topic form realsense Signed-off-by: Jakub Delicat * Made new tests Signed-off-by: Jakub Delicat * fixed astra Signed-off-by: Jakub Delicat * applied tests Signed-off-by: Jakub Delicat * Added comment about GRP03 Signed-off-by: Jakub Delicat --------- Signed-off-by: Jakub Delicat Co-authored-by: rafal-gorecki Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- config/gz_kinova_remappings.yaml | 20 +++ config/kinova_6dof_controllers.yaml | 38 ++++++ config/kinova_7dof_controllers.yaml | 39 ++++++ config/robotiq_controllers.yaml | 13 ++ config/ur_controllers.yaml | 95 ++++++++++++++ launch/gz_components.launch.py | 19 ++- launch/gz_kinova_6dof.launch.py | 182 ++++++++++++++++++++++++++ launch/gz_kinova_7dof.launch.py | 182 ++++++++++++++++++++++++++ launch/gz_robotiq.launch.py | 87 +++++++++++++ launch/gz_ur.launch.py | 102 +++++++++++++++ package.xml | 5 +- test/test_components_xacro.py | 44 ++++--- urdf/components.urdf.xacro | 117 +++++++++++++++++ urdf/kinova.urdf.xacro | 195 ++++++++++++++++++++++++++++ urdf/robotiq.urdf.xacro | 47 +++++++ urdf/ur.urdf.xacro | 170 ++++++++++++++++++++++++ 16 files changed, 1335 insertions(+), 20 deletions(-) create mode 100644 config/gz_kinova_remappings.yaml create mode 100644 config/kinova_6dof_controllers.yaml create mode 100644 config/kinova_7dof_controllers.yaml create mode 100644 config/robotiq_controllers.yaml create mode 100644 config/ur_controllers.yaml create mode 100644 launch/gz_kinova_6dof.launch.py create mode 100644 launch/gz_kinova_7dof.launch.py create mode 100644 launch/gz_robotiq.launch.py create mode 100644 launch/gz_ur.launch.py create mode 100644 urdf/kinova.urdf.xacro create mode 100644 urdf/robotiq.urdf.xacro create mode 100644 urdf/ur.urdf.xacro diff --git a/config/gz_kinova_remappings.yaml b/config/gz_kinova_remappings.yaml new file mode 100644 index 0000000..77d0510 --- /dev/null +++ b/config/gz_kinova_remappings.yaml @@ -0,0 +1,20 @@ +--- + - topic_name: /wrist_mounted_camera/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_type_name: ignition.msgs.CameraInfo + lazy: true + + - topic_name: /wrist_mounted_camera/image + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + lazy: true + + - topic_name: /wrist_mounted_camera/depth_image + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + lazy: true + + - ros_topic_name: /wrist_mounted_camera/points + ros_type_name: sensor_msgs/msg/PointCloud2 + gz_type_name: ignition.msgs.PointCloudPacked + lazy: true diff --git a/config/kinova_6dof_controllers.yaml b/config/kinova_6dof_controllers.yaml new file mode 100644 index 0000000..7990110 --- /dev/null +++ b/config/kinova_6dof_controllers.yaml @@ -0,0 +1,38 @@ +/**: + joint_trajectory_controller: + ros__parameters: + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + + twist_controller: + ros__parameters: + joint: tcp + interface_names: + - twist.linear.x + - twist.linear.y + - twist.linear.z + - twist.angular.x + - twist.angular.y + - twist.angular.z + + robotiq_gripper_controller: + ros__parameters: + default: true + joint: robotiq_85_left_knuckle_joint + allow_stalling: true diff --git a/config/kinova_7dof_controllers.yaml b/config/kinova_7dof_controllers.yaml new file mode 100644 index 0000000..af4e110 --- /dev/null +++ b/config/kinova_7dof_controllers.yaml @@ -0,0 +1,39 @@ +/**: + joint_trajectory_controller: + ros__parameters: + joints: + - joint_1 + - joint_2 + - joint_3 + - joint_4 + - joint_5 + - joint_6 + - joint_7 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.0 + goal_time: 0.0 + + twist_controller: + ros__parameters: + joint: tcp + interface_names: + - twist.linear.x + - twist.linear.y + - twist.linear.z + - twist.angular.x + - twist.angular.y + - twist.angular.z + + robotiq_gripper_controller: + ros__parameters: + default: true + joint: robotiq_85_left_knuckle_joint + allow_stalling: true 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/config/ur_controllers.yaml b/config/ur_controllers.yaml new file mode 100644 index 0000000..8a03e26 --- /dev/null +++ b/config/ur_controllers.yaml @@ -0,0 +1,95 @@ +/**: + speed_scaling_state_broadcaster: + ros__parameters: + state_publish_rate: 100.0 + + + force_torque_sensor_broadcaster: + ros__parameters: + sensor_name: tcp_fts_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + frame_id: tool0 + topic_name: ft_data + + + joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + elbow_joint: { trajectory: 0.2, goal: 0.1 } + wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + + + scaled_joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + elbow_joint: { trajectory: 0.2, goal: 0.1 } + wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + + forward_velocity_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + interface_name: velocity + + forward_position_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/launch/gz_components.launch.py b/launch/gz_components.launch.py index aaa0f70..0ff0b37 100644 --- a/launch/gz_components.launch.py +++ b/launch/gz_components.launch.py @@ -41,10 +41,11 @@ def get_launch_description(name: str, package: str, namespace: str, component: y device_namespace = get_value(component, "device_namespace") robot_namespace = namespace - if len(robot_namespace) and robot_namespace[0] != "/": - robot_namespace = "/" + robot_namespace - if len(device_namespace) and device_namespace[0] != "/": - device_namespace = "/" + device_namespace + if not name.find("ur3") and not name.find("kinova"): + if len(robot_namespace) and robot_namespace[0] != "/": + robot_namespace = "/" + robot_namespace + if len(device_namespace) and device_namespace[0] != "/": + device_namespace = "/" + device_namespace return IncludeLaunchDescription( PythonLaunchDescriptionSource([package, "/launch/gz_", name, ".launch.py"]), @@ -67,6 +68,16 @@ def get_launch_descriptions_from_yaml_node( "LDR13": "ouster_os", "LDR20": "velodyne", "CAM01": "orbbec_astra", + "MAN01": "ur", + "MAN02": "ur", + # "MAN03": "kinova_lite" sim_isaac error + "MAN04": "kinova_6dof", + "MAN05": "kinova_6dof", + "MAN06": "kinova_7dof", + "MAN07": "kinova_7dof", + "GRP02": "robotiq", + # "GRP03": "robotiq", Waiting for release + # https://github.com/PickNikRobotics/ros2_robotiq_gripper/blob/main/robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro } for component in node["components"]: diff --git a/launch/gz_kinova_6dof.launch.py b/launch/gz_kinova_6dof.launch.py new file mode 100644 index 0000000..941fa17 --- /dev/null +++ b/launch/gz_kinova_6dof.launch.py @@ -0,0 +1,182 @@ +# 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.actions import DeclareLaunchArgument, OpaqueFunction +from launch_ros.actions import Node +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ReplaceString + + +# The frame of the point cloud from ignition gazebo 6 isn't provided by . +# See https://github.com/gazebosim/gz-sensors/issues/239 +def fix_depth_image_tf(context, *args, **kwargs): + robot_namespace = LaunchConfiguration("robot_namespace").perform(context) + device_namespace = LaunchConfiguration("device_namespace").perform(context) + + device_namespace_ext = device_namespace + "/" + if device_namespace == "": + device_namespace_ext = "" + + tf_prefix_ext = device_namespace + "_" + if device_namespace == "": + tf_prefix_ext = "" + + parent_frame = tf_prefix_ext + device_namespace + "_depth_optical_frame" + child_frame = ( + "panther/base_link//" + + device_namespace_ext + + tf_prefix_ext + + device_namespace + + "_orbbec_astra_depth" + ) + + static_transform_publisher = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="point_cloud_tf", + output="log", + arguments=["0", "0", "0", "1.57", "-1.57", "0", parent_frame, child_frame], + parameters=[{"use_sim_time": True}], + namespace=robot_namespace, + ) + return [static_transform_publisher] + + +def generate_launch_description(): + robot_namespace = LaunchConfiguration("robot_namespace") + device_namespace = LaunchConfiguration("device_namespace") + + initial_joint_controllers = PathJoinSubstitution( + [FindPackageShare("ros_components_description"), "config", "kinova_6dof_controllers.yaml"] + ) + + gz_bridge_name = LaunchConfiguration("gz_bridge_name") + gz_bridge_config_path = PathJoinSubstitution( + [FindPackageShare("ros_components_description"), "config", "gz_kinova_remappings.yaml"] + ) + + namespaced_gz_bridge_config_path = ReplaceString( + source_file=gz_bridge_config_path, + replacements={ + "": robot_namespace, + "": device_namespace, + }, + ) + + # https://github.com/ros-controls/ros2_control/issues/1506 + # After this fix the device_namespace and --namespace should be used. + robot_namespace_ext = PythonExpression( + ["''", " if '", robot_namespace, "' == '' ", "else ", "'", robot_namespace, "_'"] + ) + + namespaced_initial_joint_controllers_path = ReplaceString( + source_file=initial_joint_controllers, + replacements={ + "- joint": ["- ", device_namespace, "_joint"], + " joint_trajectory_controller:": [ + " ", + robot_namespace_ext, + device_namespace, + "_joint_trajectory_controller:", + ], + " robotiq_gripper_controller:": [ + " ", + robot_namespace_ext, + device_namespace, + "_robotiq_gripper_controller:", + ], + "robotiq_85_left_knuckle_joint": [device_namespace, "_robotiq_85_left_knuckle_joint"], + }, + ) + + 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).", + ) + + # There may be other controllers of the joints, but this is the initially-started one + initial_joint_controller_spawner_started = Node( + package="controller_manager", + executable="spawner", + arguments=[ + # Using robot_namespace as prefix for controller name is caused by + # https://github.com/ros-controls/ros2_control/issues/1506 + # After this fix the device_namespace and --namespace should be used. + [robot_namespace_ext, device_namespace, "_joint_trajectory_controller"], + "-t", + "joint_trajectory_controller/JointTrajectoryController", + "-c", + "controller_manager", + "--controller-manager-timeout", + "10", + # "--namespace", + # robot_namespace, + "--param-file", + namespaced_initial_joint_controllers_path, + ], + namespace=robot_namespace, + ) + + robot_hand_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + [robot_namespace_ext, device_namespace, "_robotiq_gripper_controller"], + "-t", + "position_controllers/GripperActionController", + "-c", + "controller_manager", + "--controller-manager-timeout", + "10", + # "--namespace", + # robot_namespace, + "--param-file", + namespaced_initial_joint_controllers_path, + ], + namespace=robot_namespace, + ) + + gz_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name=gz_bridge_name, + parameters=[{"config_file": namespaced_gz_bridge_config_path}], + namespace=robot_namespace, + output="screen", + ) + + return LaunchDescription( + [ + declare_device_namespace, + declare_robot_namespace, + initial_joint_controller_spawner_started, + robot_hand_controller_spawner, + gz_bridge, + OpaqueFunction(function=fix_depth_image_tf), + ] + ) diff --git a/launch/gz_kinova_7dof.launch.py b/launch/gz_kinova_7dof.launch.py new file mode 100644 index 0000000..b57e67e --- /dev/null +++ b/launch/gz_kinova_7dof.launch.py @@ -0,0 +1,182 @@ +# 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.actions import DeclareLaunchArgument, OpaqueFunction +from launch_ros.actions import Node +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.substitutions import FindPackageShare +from nav2_common.launch import ReplaceString + + +# The frame of the point cloud from ignition gazebo 6 isn't provided by . +# See https://github.com/gazebosim/gz-sensors/issues/239 +def fix_depth_image_tf(context, *args, **kwargs): + robot_namespace = LaunchConfiguration("robot_namespace").perform(context) + device_namespace = LaunchConfiguration("device_namespace").perform(context) + + device_namespace_ext = device_namespace + "/" + if device_namespace == "": + device_namespace_ext = "" + + tf_prefix_ext = device_namespace + "_" + if device_namespace == "": + tf_prefix_ext = "" + + parent_frame = tf_prefix_ext + device_namespace + "_depth_optical_frame" + child_frame = ( + "panther/base_link//" + + device_namespace_ext + + tf_prefix_ext + + device_namespace + + "_orbbec_astra_depth" + ) + + static_transform_publisher = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="point_cloud_tf", + output="log", + arguments=["0", "0", "0", "1.57", "-1.57", "0", parent_frame, child_frame], + parameters=[{"use_sim_time": True}], + namespace=robot_namespace, + ) + return [static_transform_publisher] + + +def generate_launch_description(): + robot_namespace = LaunchConfiguration("robot_namespace") + device_namespace = LaunchConfiguration("device_namespace") + + initial_joint_controllers = PathJoinSubstitution( + [FindPackageShare("ros_components_description"), "config", "kinova_7dof_controllers.yaml"] + ) + + gz_bridge_name = LaunchConfiguration("gz_bridge_name") + gz_bridge_config_path = PathJoinSubstitution( + [FindPackageShare("ros_components_description"), "config", "gz_kinova_remappings.yaml"] + ) + + namespaced_gz_bridge_config_path = ReplaceString( + source_file=gz_bridge_config_path, + replacements={ + "": robot_namespace, + "": device_namespace, + }, + ) + + # https://github.com/ros-controls/ros2_control/issues/1506 + # After this fix the device_namespace and --namespace should be used. + robot_namespace_ext = PythonExpression( + ["''", " if '", robot_namespace, "' == '' ", "else ", "'", robot_namespace, "_'"] + ) + + namespaced_initial_joint_controllers_path = ReplaceString( + source_file=initial_joint_controllers, + replacements={ + "- joint": ["- ", device_namespace, "_joint"], + " joint_trajectory_controller:": [ + " ", + robot_namespace_ext, + device_namespace, + "_joint_trajectory_controller:", + ], + " robotiq_gripper_controller:": [ + " ", + robot_namespace_ext, + device_namespace, + "_robotiq_gripper_controller:", + ], + "robotiq_85_left_knuckle_joint": [device_namespace, "_robotiq_85_left_knuckle_joint"], + }, + ) + + 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).", + ) + + # There may be other controllers of the joints, but this is the initially-started one + initial_joint_controller_spawner_started = Node( + package="controller_manager", + executable="spawner", + arguments=[ + # Using robot_namespace as prefix for controller name is caused by + # https://github.com/ros-controls/ros2_control/issues/1506 + # After this fix the device_namespace and --namespace should be used. + [robot_namespace_ext, device_namespace, "_joint_trajectory_controller"], + "-t", + "joint_trajectory_controller/JointTrajectoryController", + "-c", + "controller_manager", + "--controller-manager-timeout", + "10", + # "--namespace", + # robot_namespace, + "--param-file", + namespaced_initial_joint_controllers_path, + ], + namespace=robot_namespace, + ) + + robot_hand_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + [robot_namespace_ext, device_namespace, "_robotiq_gripper_controller"], + "-t", + "position_controllers/GripperActionController", + "-c", + "controller_manager", + "--controller-manager-timeout", + "10", + # "--namespace", + # robot_namespace, + "--param-file", + namespaced_initial_joint_controllers_path, + ], + namespace=robot_namespace, + ) + + gz_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name=gz_bridge_name, + parameters=[{"config_file": namespaced_gz_bridge_config_path}], + namespace=robot_namespace, + output="screen", + ) + + return LaunchDescription( + [ + declare_device_namespace, + declare_robot_namespace, + initial_joint_controller_spawner_started, + robot_hand_controller_spawner, + gz_bridge, + OpaqueFunction(function=fix_depth_image_tf), + ] + ) diff --git a/launch/gz_robotiq.launch.py b/launch/gz_robotiq.launch.py new file mode 100644 index 0000000..e5caebf --- /dev/null +++ b/launch/gz_robotiq.launch.py @@ -0,0 +1,87 @@ +# 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.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution, PythonExpression +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") + + initial_joint_controllers = PathJoinSubstitution( + [FindPackageShare("ros_components_description"), "config", "robotiq_controllers.yaml"] + ) + + + # Using robot_namespace as prefix for controller name is caused by + # https://github.com/ros-controls/ros2_control/issues/1506 + # After this fix the device_namespace and --namespace should be used. + robot_namespace_ext = PythonExpression( + ["''", " if '", robot_namespace, "' == '' ", "else ", "'", robot_namespace, "_'"] + ) + + namespaced_initial_joint_controllers_path = ReplaceString( + source_file=initial_joint_controllers, + replacements={ + "robotiq_85_left_knuckle_joint": [device_namespace, "_robotiq_85_left_knuckle_joint"], + " robotiq_gripper_controller:": [" ", robot_namespace_ext, device_namespace, "_robotiq_gripper_controller:"], + " robotiq_activation_controller:": [" ", robot_namespace_ext, device_namespace, "_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).", + ) + + robotiq_gripper_controller = Node( + package="controller_manager", + executable="spawner", + arguments=[ + # Using robot_namespace as prefix for controller name is caused by + # https://github.com/ros-controls/ros2_control/issues/1506 + # After this fix the device_namespace and --namespace should be used. + [robot_namespace_ext , device_namespace, "_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, + robotiq_gripper_controller, + ] + ) diff --git a/launch/gz_ur.launch.py b/launch/gz_ur.launch.py new file mode 100644 index 0000000..8781d10 --- /dev/null +++ b/launch/gz_ur.launch.py @@ -0,0 +1,102 @@ +# 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.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from launch.substitutions import ( + EnvironmentVariable, + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +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") + initial_joint_controllers = PathJoinSubstitution( + [FindPackageShare("ros_components_description"), "config", "ur_controllers.yaml"] + ) + + # Using robot_namespace as prefix for controller name is caused by + # https://github.com/ros-controls/ros2_control/issues/1506 + # After this fix the device_namespace and --namespace should be used. + robot_namespace_ext = PythonExpression( + ["''", " if '", robot_namespace, "' == '' ", "else ", "'", robot_namespace, "_'"] + ) + + namespaced_initial_joint_controllers_path = ReplaceString( + source_file=initial_joint_controllers, + replacements={ + "shoulder_pan_joint": [device_namespace, "_shoulder_pan_joint"], + "shoulder_lift_joint": [device_namespace, "_shoulder_lift_joint"], + "elbow_joint": [device_namespace, "_elbow_joint"], + "wrist_1_joint": [device_namespace, "_wrist_1_joint"], + "wrist_2_joint": [device_namespace, "_wrist_2_joint"], + "wrist_3_joint": [device_namespace, "_wrist_3_joint"], + "tool0": [device_namespace, "_tool0"], + " joint_trajectory_controller:": [ + " ", + robot_namespace_ext, + device_namespace, + "_joint_trajectory_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).", + ) + + # There may be other controllers of the joints, but this is the initially-started one + initial_joint_controller_spawner_started = Node( + package="controller_manager", + executable="spawner", + arguments=[ + # Using robot_namespace as prefix for controller name is caused by + # https://github.com/ros-controls/ros2_control/issues/1506 + # After this fix the device_namespace and --namespace should be used. + [robot_namespace_ext, device_namespace, "_joint_trajectory_controller"], + "-t", + "joint_trajectory_controller/JointTrajectoryController", + "-c", + "controller_manager", + "--controller-manager-timeout", + "10", + # "--namespace", + # robot_namespace, + "--param-file", + namespaced_initial_joint_controllers_path, + ], + namespace=robot_namespace, + ) + + return LaunchDescription( + [ + declare_device_namespace, + declare_robot_namespace, + initial_joint_controller_spawner_started, + ] + ) diff --git a/package.xml b/package.xml index ca2280a..202fbbe 100644 --- a/package.xml +++ b/package.xml @@ -20,12 +20,15 @@ robot_state_publisher urdf xacro + ur_description + kortex_description + robotiq_description ros_gz_sim ros_gz_bridge launch nav2_common - + ament_index_python ament_cmake_pytest python3-yaml diff --git a/test/test_components_xacro.py b/test/test_components_xacro.py index 7b0f066..60e4bdb 100644 --- a/test/test_components_xacro.py +++ b/test/test_components_xacro.py @@ -24,13 +24,23 @@ ros_components_description = get_package_share_directory("ros_components_description") xacro_path = os.path.join(ros_components_description, "test/component.urdf.xacro") -# Type: [model_link, sensor_link, sensor_name] +# Type: [model_link, link_name, sensor_link_name, sensor_name] components_types_with_names = { - "LDR01": ["slamtec_rplidar_s1", "laser", "slamtec_rplidar_s1_sensor"], - "LDR06": ["slamtec_rplidar_s3", "laser", "slamtec_rplidar_s3_sensor"], - "LDR13": ["ouster_os1_32", "os_lidar", "ouster_os1_32_sensor"], - "LDR20": ["velodyne_puck", "velodyne", "velodyne_puck_sensor"], - "CAM01": ["orbbec_astra", "link", "orbbec_astra_color"], + "LDR01": ["slamtec_rplidar_s1", "laser", "laser", "slamtec_rplidar_s1_sensor"], + "LDR06": ["slamtec_rplidar_s3", "laser", "laser", "slamtec_rplidar_s3_sensor"], + "LDR13": ["ouster_os1_32", "os_lidar", "os_lidar", "ouster_os1_32_sensor"], + "LDR20": ["velodyne_puck", "velodyne", "velodyne", "velodyne_puck_sensor"], + "CAM01": ["orbbec_astra", "link", "link", "orbbec_astra_color"], + "MAN01": ["ur3e", "base_link", "", ""], + "MAN02": ["ur5e", "base_link", "", ""], + # "MAN03": ["kinova_lite", "base_link", "", ""], use_isaac error + "MAN04": ["kinova_gen3_6dof", "base_link", "", ""], + "MAN05": ["kinova_gen3_6dof", "base_link", "camera_color_frame", "camera_sensor"], + "MAN06": ["kinova_gen3_7dof", "base_link", "", ""], + "MAN07": ["kinova_gen3_7dof", "base_link", "camera_color_frame", "camera_sensor"], + # "GRP01": [], not implemented in robotiq_description + "GRP02": ["robotiq", "robotiq_85_base_link", "", ""], + # "GRP03": ["robotiq", "robotiq_140_base_link", "", ""], not implemented in robotiq_description } @@ -93,31 +103,35 @@ def does_sensor_name_exist( def test_component(self, component: dict, expected_result: list, components_config_path: str): names = components_types_with_names[component["type"]] component_name = names[0] - sensor_reference = names[1] - sensor_name = names[2] + link_name = names[1] + sensor_link_name = names[2] + sensor_name = names[3] device_namespace = component["device_namespace"] - link_name = device_namespace + "_" + component_name + "_link" - sensor_name = device_namespace + "_" + sensor_name - sensor_link_name = device_namespace + "_" + sensor_reference + namespaced_link_name = device_namespace + "_" + link_name + namespaced_sensor_link_name = device_namespace + "_" + sensor_link_name + namespaced_sensor_name = device_namespace + "_" + sensor_name if self.does_urdf_parse() != expected_result[0]: assert ( False ), f"Expected prase result {expected_result[0]} with file {components_config_path} and component {component_name}." - if self.does_link_exist(self._urdf, link_name) != expected_result[1]: + if self.does_link_exist(self._urdf, namespaced_link_name) != expected_result[1]: assert ( False - ), f"Link name: {link_name}. Expected result {expected_result[1]} with file {components_config_path} and component {component_name}." + ), f"Link name: {namespaced_link_name}. Expected result {expected_result[1]} with file {components_config_path} and component {component_name}." if ( - self.does_sensor_name_exist(self._urdf, sensor_link_name, sensor_name) + names[2] != "" + and self.does_sensor_name_exist( + self._urdf, namespaced_sensor_link_name, namespaced_sensor_name + ) != expected_result[2] ): assert ( False - ), f"Sensor name: {sensor_name}, sensor link name: {sensor_link_name}. Expected result {expected_result[2]} with file {components_config_path} and component {component_name}." + ), f"Sensor name: {namespaced_sensor_name}, sensor link name: {namespaced_sensor_link_name}. Expected result {expected_result[2]} with file {components_config_path} and component {component_name}." def test_all_good_single_components(tmpdir_factory): diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro index ac3d727..8a58045 100644 --- a/urdf/components.urdf.xacro +++ b/urdf/components.urdf.xacro @@ -101,6 +101,123 @@ /> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1.047 + + ${camera_width} + ${camera_height} + RGB_INT8 + + + 0.1 + 5 + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + + 554.25469 + 554.25469 + 320.5 + 240.5 + 0 + + + + 554.25469 + 554.25469 + 320.5 + 240.5 + 0 + 0 + + + + gaussian + 0 + 0.00 + + + + 0.25 + 5 + + + ${device_namespace}_color_optical_frame + + ${device_namespace}_camera_color_frame + 1 + ${camera_fps} + true + ${ns}wrist_mounted_camera + false + + + + + + + diff --git a/urdf/robotiq.urdf.xacro b/urdf/robotiq.urdf.xacro new file mode 100644 index 0000000..b77f49e --- /dev/null +++ b/urdf/robotiq.urdf.xacro @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/ur.urdf.xacro b/urdf/ur.urdf.xacro new file mode 100644 index 0000000..9718127 --- /dev/null +++ b/urdf/ur.urdf.xacro @@ -0,0 +1,170 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + {-2*pi} + {2*pi} + + + -3.15 + 3.15 + + + + ${initial_positions['shoulder_pan_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.15 + 3.15 + + + + ${initial_positions['shoulder_lift_joint']} + + + + + + + {-pi} + {pi} + + + -3.15 + 3.15 + + + + ${initial_positions['elbow_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + + ${initial_positions['wrist_1_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + + ${initial_positions['wrist_2_joint']} + + + + + + + {-2*pi} + {2*pi} + + + -3.2 + 3.2 + + + + ${initial_positions['wrist_3_joint']} + + + + + + + +