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 @@ /> - + + + @@ -60,7 +67,7 @@ rpy="${rpy}" namespace="${ns}" device_namespace="${device_namespace}" - model="zed2i" + model="${zed_models_dict[type]}" />