From dad33238d3f31aa57479f940636ba77d4752710e Mon Sep 17 00:00:00 2001
From: Jakub Delicat <jakub.delicat@husarion.com>
Date: Wed, 12 Jun 2024 09:42:07 +0200
Subject: [PATCH] Added zed

Signed-off-by: Jakub Delicat <jakub.delicat@husarion.com>
---
 launch/gz_components.launch.py     |  6 ++---
 launch/gz_stereolabs_zed.launch.py | 36 +++++++++---------------------
 urdf/components.urdf.xacro         | 11 +++++++--
 3 files changed, 22 insertions(+), 31 deletions(-)

diff --git a/launch/gz_components.launch.py b/launch/gz_components.launch.py
index bf41520..9bbbabd 100644
--- a/launch/gz_components.launch.py
+++ b/launch/gz_components.launch.py
@@ -73,9 +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",
+        "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
index 079349c..ebb799e 100644
--- a/launch/gz_stereolabs_zed.launch.py
+++ b/launch/gz_stereolabs_zed.launch.py
@@ -26,30 +26,22 @@
 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)
-
-    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 + + "_depth_optical_frame"
-    child_frame = (
-        "panther/base_link//"
-        + device_namespace_ext
-        + tf_prefix_ext
-        + "_orbbec_astra_depth"
-    )
+
+    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", "1.57", "-1.57", "0", parent_frame, child_frame],
+        arguments=["0", "0", "0", "0", "0", "0", parent_frame, child_frame],
         parameters=[{"use_sim_time": True}],
         namespace=robot_namespace,
     )
@@ -64,7 +56,6 @@ def generate_launch_description():
 
     robot_namespace = LaunchConfiguration("robot_namespace")
     device_namespace = LaunchConfiguration("device_namespace")
-    camera_name = LaunchConfiguration("camera_name")
     gz_bridge_name = LaunchConfiguration("gz_bridge_name")
 
     namespaced_gz_bridge_config_path = ReplaceString(
@@ -81,12 +72,6 @@ def generate_launch_description():
         description="Sensor namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.",
     )
 
-    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_robot_namespace = DeclareLaunchArgument(
         "robot_namespace",
         default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
@@ -112,7 +97,6 @@ def generate_launch_description():
         [
             declare_device_namespace,
             declare_robot_namespace,
-            declare_tf_prefix,
             declare_gz_bridge_name,
             gz_bridge,
             OpaqueFunction(function=fix_depth_image_tf),
diff --git a/urdf/components.urdf.xacro b/urdf/components.urdf.xacro
index d67b598..203e9b1 100644
--- a/urdf/components.urdf.xacro
+++ b/urdf/components.urdf.xacro
@@ -49,7 +49,14 @@
         />
       </xacro:if>
 
-      <xacro:if value="${type == 'CAM04'}">
+      <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" />
@@ -60,7 +67,7 @@
           rpy="${rpy}"
           namespace="${ns}"
           device_namespace="${device_namespace}"
-          model="zed2i"
+          model="${zed_models_dict[type]}"
         />
       </xacro:if>