update launches

Signed-off-by: Jakub Delicat <[email protected]>

launch/CAM01.launch.py

@@ -14,24 +14,57 @@
 
 import os
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-)
-
+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)
+    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():
     ros_components_description = get_package_share_directory("ros_components_description")
     gz_bridge_config_path = os.path.join(
         ros_components_description, "config", "gz_orbbec_astra_remappings.yaml"
     )
 
     robot_namespace = LaunchConfiguration("robot_namespace")
-    tf_prefix = LaunchConfiguration("tf_prefix")
     device_namespace = LaunchConfiguration("device_namespace")
     camera_name = LaunchConfiguration("camera_name")
     gz_bridge_name = LaunchConfiguration("gz_bridge_name")
@@ -84,47 +117,6 @@ def generate_launch_description():
         output="screen",
     )
 
-    # 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
-    #  panther/base_link//front_cam_ns/front_cam_tf_camera_depth_optical_frame
-    # panther/base_link//front_cam_ns/front_cam_tf_camera_depth_optical_frame
-    # panther/base_link//front_cam_ns/front_cam_tf_camera_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",
-            LaunchConfiguration(
-                "parent_frame", default=[tf_prefix, "_", camera_name, "_depth_optical_frame"]
-            ),
-            LaunchConfiguration(
-                "child_frame",
-                default=[
-                    "panther/base_link//",
-                    device_namespace,
-                    "/",
-                    tf_prefix,
-                    "_",
-                    camera_name,
-                    "_orbbec_astra_depth",
-                ],
-            ),
-        ],
-        remappings=[
-            ("/tf", "tf"),
-            ("/tf_static", "tf_static"),
-        ],
-        parameters=[{"use_sim_time": True}],
-        namespace=robot_namespace,
-    )
-
     return LaunchDescription(
         [
             declare_device_namespace,
@@ -133,6 +125,6 @@ def generate_launch_description():
             declare_camera_name,
             declare_gz_bridge_name,
             gz_bridge,
-            static_transform_publisher,
+            OpaqueFunction(function=fix_depth_image_tf),
         ]
     )

launch/LDR01.launch.py

@@ -14,13 +14,9 @@
 
 import os
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-)
-
+from launch.actions import DeclareLaunchArgument
 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
 

launch/LDR06.launch.py

@@ -14,10 +14,7 @@
 
 import os
 from launch import LaunchDescription
-from launch.actions import (
-    DeclareLaunchArgument,
-)
-
+from launch.actions import DeclareLaunchArgument
 from launch_ros.actions import Node
 from launch.substitutions import EnvironmentVariable, LaunchConfiguration
 from nav2_common.launch import ReplaceString

launch/gz_components.launch.py

@@ -14,7 +14,6 @@
 
 import os
 import yaml
-
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
@@ -27,30 +26,34 @@
 from launch.substitutions import LaunchConfiguration, EnvironmentVariable
 
 
-def get_value_or_none(node: yaml.Node, key: str):
+def get_value(node: yaml.Node, key: str):
     try:
-        return node[key]
+        value = node[key]
+        if value == "None":
+            value = ""
+        print(f"VALUUUUUUUE node[{key}] = {value}")
+        return value
+
     except KeyError:
-        return 'None'
+        return ""
 
 
 def get_launch_descriptions_from_yaml_node(
     node: yaml.Node, package: os.PathLike, namespace: str
 ) -> IncludeLaunchDescription:
     actions = []
     for component in node["components"]:
-
         actions.append(
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
                     [package, "/launch/", component["type"], ".launch.py"]
                 ),
                 launch_arguments={
                     "robot_namespace": namespace,
-                    "device_namespace": get_value_or_none(component, "namespace"),
-                    "tf_prefix": get_value_or_none(component, "tf_prefix"),
+                    "device_namespace": get_value(component, "namespace"),
+                    "tf_prefix": get_value(component, "tf_prefix"),
                     "gz_bridge_name": component["namespace"][1:] + "_gz_bridge",
-                    "camera_name": get_value_or_none(component, "name"),
+                    "camera_name": get_value(component, "name"),
                 }.items(),
             )
         )