From a6473588eee97feaee3481f9eba4f746b1f4476e Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Wed, 12 Jun 2024 11:05:34 +0200 Subject: [PATCH] ROS 2 ZED Gazebo Components fix (#53) * No namespaces fix Signed-off-by: Jakub Delicat * Add lazy, fix and simplify orbbec_astra, remove from * Delete unused arg * Rebase branch * WIP zed Signed-off-by: Jakub Delicat * WIP Zed Signed-off-by: Jakub Delicat * revert astra resolution Signed-off-by: Jakub Delicat * without namespace working Signed-off-by: Jakub Delicat * Added zed Signed-off-by: Jakub Delicat * Tests Signed-off-by: Jakub Delicat * added CR Signed-off-by: Jakub Delicat --------- Signed-off-by: Jakub Delicat Co-authored-by: rafal-gorecki --- config/gz_stereolabs_zed_remappings.yaml | 31 +++++++ launch/gz_components.launch.py | 3 + launch/gz_stereolabs_zed.launch.py | 104 +++++++++++++++++++++++ test/test_components_xacro.py | 3 + urdf/components.urdf.xacro | 22 +++++ urdf/stereolabs_zed.urdf.xacro | 3 +- 6 files changed, 165 insertions(+), 1 deletion(-) create mode 100644 config/gz_stereolabs_zed_remappings.yaml create mode 100644 launch/gz_stereolabs_zed.launch.py diff --git a/config/gz_stereolabs_zed_remappings.yaml b/config/gz_stereolabs_zed_remappings.yaml new file mode 100644 index 0000000..8d10d3a --- /dev/null +++ b/config/gz_stereolabs_zed_remappings.yaml @@ -0,0 +1,31 @@ +--- +# https://github.com/stereolabs/zed-ros2-wrapper +- ros_topic_name: /zed_node/rgb/camera_info + gz_topic_name: /zed_node/rgb/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_type_name: ignition.msgs.CameraInfo + lazy: true + +- ros_topic_name: /zed_node/rgb/image_rect_color + gz_topic_name: /zed_node/rgb/image_rect_color + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + lazy: true + +- ros_topic_name: /zed_node/camera_info + gz_topic_name: /zed_node/camera_info + ros_type_name: sensor_msgs/msg/CameraInfo + gz_type_name: ignition.msgs.CameraInfo + lazy: true + +- ros_topic_name: /zed_node/depth + gz_topic_name: /zed_node/depth + ros_type_name: sensor_msgs/msg/Image + gz_type_name: ignition.msgs.Image + lazy: true + +- ros_topic_name: /zed_node/depth/points + gz_ros_topic_name: /zed_node/depth/points + ros_type_name: sensor_msgs/msg/PointCloud2 + gz_type_name: ignition.msgs.PointCloudPacked + lazy: true diff --git a/launch/gz_components.launch.py b/launch/gz_components.launch.py index 98980a5..9bbbabd 100644 --- a/launch/gz_components.launch.py +++ b/launch/gz_components.launch.py @@ -73,6 +73,9 @@ def get_launch_descriptions_from_yaml_node( "LDR15": "ouster_os", "LDR20": "velodyne", "CAM01": "orbbec_astra", + "CAM03": "stereolabs_zed", + "CAM04": "stereolabs_zed", + "CAM06": "stereolabs_zed", "MAN01": "ur", "MAN02": "ur", # "MAN03": "kinova_lite" sim_isaac error diff --git a/launch/gz_stereolabs_zed.launch.py b/launch/gz_stereolabs_zed.launch.py new file mode 100644 index 0000000..ebb799e --- /dev/null +++ b/launch/gz_stereolabs_zed.launch.py @@ -0,0 +1,104 @@ +# 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. + +import os +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch_ros.actions import Node +from launch.substitutions import EnvironmentVariable, LaunchConfiguration +from nav2_common.launch import ReplaceString +from ament_index_python import get_package_share_directory + + +# 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) + + if robot_namespace.startswith('/'): + robot_namespace = robot_namespace[1:] + "/" + + if device_namespace.startswith('/'): + device_namespace = device_namespace[1:] + + parent_frame = robot_namespace + device_namespace + "_center_optical_frame" + child_frame = "panther/base_link/" + robot_namespace + device_namespace + "_stereolabs_zed_depth" + + static_transform_publisher = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="point_cloud_tf", + output="log", + arguments=["0", "0", "0", "0", "0", "0", parent_frame, child_frame], + parameters=[{"use_sim_time": True}], + namespace=robot_namespace, + ) + return [static_transform_publisher] + + +def generate_launch_description(): + ros_components_description = get_package_share_directory("ros_components_description") + gz_bridge_config_path = os.path.join( + ros_components_description, "config", "gz_stereolabs_zed_remappings.yaml" + ) + + robot_namespace = LaunchConfiguration("robot_namespace") + device_namespace = LaunchConfiguration("device_namespace") + gz_bridge_name = LaunchConfiguration("gz_bridge_name") + + namespaced_gz_bridge_config_path = ReplaceString( + source_file=gz_bridge_config_path, + replacements={ + "": robot_namespace, + "": device_namespace, + }, + ) + + 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_gz_bridge_name = DeclareLaunchArgument( + "gz_bridge_name", + default_value="gz_bridge", + description="Name of gz bridge node.", + ) + + 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_gz_bridge_name, + gz_bridge, + OpaqueFunction(function=fix_depth_image_tf), + ] + ) diff --git a/test/test_components_xacro.py b/test/test_components_xacro.py index a480e65..8df4c01 100644 --- a/test/test_components_xacro.py +++ b/test/test_components_xacro.py @@ -36,6 +36,9 @@ "LDR15": ["ouster_os1_128", "os_lidar", "os_lidar", "ouster_os1_128_sensor"], "LDR20": ["velodyne_puck", "velodyne", "velodyne", "velodyne_puck_sensor"], "CAM01": ["orbbec_astra", "link", "link", "orbbec_astra_color"], + "CAM03": ["zed2", "center", "center_optical_frame", "stereolabs_zed_depth"], + "CAM04": ["zed2i", "center", "center_optical_frame", "stereolabs_zed_depth"], + "CAM06": ["zedi", "center", "center_optical_frame", "stereolabs_zed_depth"], "MAN01": ["ur3e", "base_link", "", ""], "MAN02": ["ur5e", "base_link", "", ""], # "MAN03": ["kinova_lite", "base_link", "", ""], use_isaac error diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro index b92e621..203e9b1 100644 --- a/urdf/components.urdf.xacro +++ b/urdf/components.urdf.xacro @@ -49,6 +49,28 @@ /> + + + + + + + +