ROS 2 ZED Gazebo Components fix (#53)

* No namespaces fix Signed-off-by: Jakub Delicat <[email protected]> * Add lazy, fix and simplify orbbec_astra, remove <ros> from <sensor> * Delete unused arg * Rebase branch * WIP zed Signed-off-by: Jakub Delicat <[email protected]> * WIP Zed Signed-off-by: Jakub Delicat <[email protected]> * revert astra resolution Signed-off-by: Jakub Delicat <[email protected]> * without namespace working Signed-off-by: Jakub Delicat <[email protected]> * Added zed Signed-off-by: Jakub Delicat <[email protected]> * Tests Signed-off-by: Jakub Delicat <[email protected]> * added CR Signed-off-by: Jakub Delicat <[email protected]> --------- Signed-off-by: Jakub Delicat <[email protected]> Co-authored-by: rafal-gorecki <[email protected]>
husarion · Jun 12, 2024 · a647358 · a647358
1 parent 20b5c9b
commit a647358
Show file tree

Hide file tree

Showing 6 changed files with 165 additions and 1 deletion.
diff --git a/config/gz_stereolabs_zed_remappings.yaml b/config/gz_stereolabs_zed_remappings.yaml
@@ -0,0 +1,31 @@
+---
+# https://github.com/stereolabs/zed-ros2-wrapper
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/camera_info
+  gz_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/camera_info
+  ros_type_name: sensor_msgs/msg/CameraInfo
+  gz_type_name: ignition.msgs.CameraInfo
+  lazy: true
+
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/image_rect_color
+  gz_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/image_rect_color
+  ros_type_name: sensor_msgs/msg/Image
+  gz_type_name: ignition.msgs.Image
+  lazy: true
+
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/camera_info
+  gz_topic_name: <robot_namespace><device_namespace>/zed_node/camera_info
+  ros_type_name: sensor_msgs/msg/CameraInfo
+  gz_type_name: ignition.msgs.CameraInfo
+  lazy: true
+
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth
+  gz_topic_name: <robot_namespace><device_namespace>/zed_node/depth
+  ros_type_name: sensor_msgs/msg/Image
+  gz_type_name: ignition.msgs.Image
+  lazy: true
+
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth/points
+  gz_ros_topic_name: <robot_namespace><device_namespace>/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
@@ -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
@@ -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 <frame_id>.
+# 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>": robot_namespace,
+            "<device_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
@@ -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
@@ -49,6 +49,28 @@
         />
       </xacro:if>
 
+      <xacro:property name="zed_models_dict"
+        value="${dict ([
+        ('CAM03', 'zed2'),
+        ('CAM04', 'zed2i'),
+        ('CAM06', 'zedx'),
+        ])}" />
+
+        <xacro:if value="${type in zed_models_dict}">
+        <xacro:include
+          filename="$(find ros_components_description)/urdf/stereolabs_zed.urdf.xacro"
+          ns="camera" />
+
+        <xacro:camera.zed_camera
+          parent_link="${parent_link}"
+          xyz="${xyz}"
+          rpy="${rpy}"
+          namespace="${ns}"
+          device_namespace="${device_namespace}"
+          model="${zed_models_dict[type]}"
+        />
+      </xacro:if>
+
       <xacro:if value="${type == 'LDR01'}">
         <xacro:include
           filename="$(find ros_components_description)/urdf/slamtec_rplidar_s1.urdf.xacro"

diff --git a/urdf/stereolabs_zed.urdf.xacro b/urdf/stereolabs_zed.urdf.xacro
@@ -1,6 +1,7 @@
 <?xml version="1.0"?>
 
 <!--
+// Copyright 2024 Husarion sp. z o.o.
 // Copyright 2022 Stereolabs
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
@@ -106,7 +107,7 @@
         </geometry>
       </collision>
     </link>
-    <joint name="${device_namespace}_center_joint" type="fixed">
+    <joint name="${device_namespace}_camera_center_joint" type="fixed">
       <parent link="${device_namespace}_link" />
       <child link="${device_namespace}_center" />
       <origin xyz="0 0 ${height/2}" rpy="0 ${bottom_slope} 0" />