From 17806b9c2841077f6ca402e3c320f5f5246f66a4 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 26 Apr 2024 17:36:57 +0200 Subject: [PATCH] added kinova Signed-off-by: Jakub Delicat --- config/gz_kinova_remappings.yaml | 20 ++++ config/kinova_controllers.yaml | 39 ++++++ launch/gz_components.launch.py | 12 +- launch/gz_kinova.launch.py | 200 +++++++++++++++++++++++++++++++ launch/gz_ur.launch.py | 25 ---- package.xml | 1 + urdf/components.urdf.xacro | 75 ++++++++++++ urdf/kinova.urdf.xacro | 195 ++++++++++++++++++++++++++++++ 8 files changed, 539 insertions(+), 28 deletions(-) create mode 100644 config/gz_kinova_remappings.yaml create mode 100644 config/kinova_controllers.yaml create mode 100644 launch/gz_kinova.launch.py create mode 100644 urdf/kinova.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_controllers.yaml b/config/kinova_controllers.yaml new file mode 100644 index 0000000..af4e110 --- /dev/null +++ b/config/kinova_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/launch/gz_components.launch.py b/launch/gz_components.launch.py index d5a2e85..6d22b30 100644 --- a/launch/gz_components.launch.py +++ b/launch/gz_components.launch.py @@ -67,11 +67,17 @@ def get_launch_descriptions_from_yaml_node( if component["type"] == "CAM01": actions.append(get_launch_description("orbbec_astra", package, namespace, component)) - if component["type"] == "MAN01": + if component["type"] == "MAN01" or component["type"] == "MAN02": actions.append(get_launch_description("ur", package, namespace, component)) - if component["type"] == "MAN02": - actions.append(get_launch_description("ur", package, namespace, component)) + if ( + component["type"] == "MAN04" + # or component["type"] == "MAN03" # sim_isaac error + or component["type"] == "MAN05" + or component["type"] == "MAN06" + or component["type"] == "MAN07" + ): + actions.append(get_launch_description("kinova", package, namespace, component)) return actions diff --git a/launch/gz_kinova.launch.py b/launch/gz_kinova.launch.py new file mode 100644 index 0000000..1c9cec5 --- /dev/null +++ b/launch/gz_kinova.launch.py @@ -0,0 +1,200 @@ +# 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 LaunchConfigurationNotEquals +from launch.actions import DeclareLaunchArgument, LogInfo, OpaqueFunction +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 + + +# 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) + tf_prefix = LaunchConfiguration("tf_prefix").perform(context) + camera_name = LaunchConfiguration("camera_name").perform(context) + + device_namespace_ext = device_namespace + "/" + if device_namespace == "": + device_namespace_ext = "" + + tf_prefix_ext = tf_prefix + "_" + if tf_prefix == "": + tf_prefix_ext = "" + + parent_frame = tf_prefix_ext + camera_name + "_depth_optical_frame" + child_frame = ( + "panther/base_link//" + + device_namespace_ext + + tf_prefix_ext + + camera_name + + "_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") + tf_prefix = LaunchConfiguration("tf_prefix") + controllers_file = LaunchConfiguration("controllers_file") + + initial_joint_controllers = PathJoinSubstitution( + [FindPackageShare("ros_components_description"), "config", controllers_file] + ) + + 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"), + ) + + 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, + }, + ) + + # 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={ + "- joint": ["- ", tf_prefix, "joint"], + " joint_trajectory_controller:": [" ", tf_prefix, "joint_trajectory_controller:"], + " robotiq_gripper_controller:": [" ", tf_prefix, "robotiq_gripper_controller:"], + "robotiq_85_left_knuckle_joint": [tf_prefix, "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).", + ) + + declare_tf_prefix = DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="Prefix added for all links of device. Here used as fix for static transform publisher.", + ) + + declare_start_joint_controller = DeclareLaunchArgument( + "start_joint_controller", + default_value="true", + description="Enable headless mode for robot control", + ) + + declare_initial_joint_controller = DeclareLaunchArgument( + "initial_joint_controller", + default_value=[tf_prefix, "joint_trajectory_controller"], + description="Robot controller to start.", + ) + + declare_controllers_file = DeclareLaunchArgument( + "controllers_file", + default_value="kinova_controllers.yaml", + description="YAML file with the controllers configuration.", + ) + + # 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=[ + [tf_prefix, "joint_trajectory_controller"], + "-t", + "joint_trajectory_controller/JointTrajectoryController", + "-c", + "controller_manager", + "--controller-manager-timeout", + "10", + "--namespace", + device_namespace, + "--param-file", + namespaced_initial_joint_controllers_path, + ], + namespace=robot_namespace, + ) + + robot_hand_controller_spawner = 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, + ) + + 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, + declare_tf_prefix, + declare_start_joint_controller, + declare_initial_joint_controller, + declare_controllers_file, + namespace_warn, + initial_joint_controller_spawner_started, + robot_hand_controller_spawner, + gz_bridge, + OpaqueFunction(function=fix_depth_image_tf), + ] + ) diff --git a/launch/gz_ur.launch.py b/launch/gz_ur.launch.py index 108cb30..531981d 100644 --- a/launch/gz_ur.launch.py +++ b/launch/gz_ur.launch.py @@ -24,8 +24,6 @@ def generate_launch_description(): robot_namespace = LaunchConfiguration("robot_namespace") device_namespace = LaunchConfiguration("device_namespace") tf_prefix = LaunchConfiguration("tf_prefix") - start_joint_controller = LaunchConfiguration("start_joint_controller") - initial_joint_controller = LaunchConfiguration("initial_joint_controller") controllers_file = LaunchConfiguration("controllers_file") initial_joint_controllers = PathJoinSubstitution( @@ -72,18 +70,6 @@ def generate_launch_description(): description="Prefix added for all links of device. Here used as fix for static transform publisher.", ) - declare_start_joint_controller = DeclareLaunchArgument( - "start_joint_controller", - default_value="true", - description="Enable headless mode for robot control", - ) - - declare_initial_joint_controller = DeclareLaunchArgument( - "initial_joint_controller", - default_value=[tf_prefix, "joint_trajectory_controller"], - description="Robot controller to start.", - ) - declare_controllers_file = DeclareLaunchArgument( "controllers_file", default_value="ur_controllers.yaml", @@ -108,14 +94,6 @@ def generate_launch_description(): namespaced_initial_joint_controllers_path, ], namespace=robot_namespace, - condition=IfCondition(start_joint_controller), - ) - - initial_joint_controller_spawner_stopped = Node( - package="controller_manager", - executable="spawner", - arguments=[initial_joint_controller, "-c", "/controller_manager", "--stopped"], - condition=UnlessCondition(start_joint_controller), ) return LaunchDescription( @@ -123,11 +101,8 @@ def generate_launch_description(): declare_device_namespace, declare_robot_namespace, declare_tf_prefix, - declare_start_joint_controller, - declare_initial_joint_controller, declare_controllers_file, namespace_warn, initial_joint_controller_spawner_started, - initial_joint_controller_spawner_stopped, ] ) diff --git a/package.xml b/package.xml index a2e1c78..c82c31a 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,7 @@ urdf xacro ur_description + kortex_description gazebo_plugins diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro index 1ab0a63..b8f5541 100644 --- a/urdf/components.urdf.xacro +++ b/urdf/components.urdf.xacro @@ -162,6 +162,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 + + + ${tf_prefix}color_optical_frame + + ${tf_prefix}camera_color_frame + 1 + ${camera_fps} + true + ${ns}wrist_mounted_camera + false + + + + + + +