publish tfs and set matching frame names

ros-drivers · Nov 12, 2023 · 0f919b8 · 0f919b8
1 parent e07df4e
commit 0f919b8
Showing 1 changed file with 30 additions and 3 deletions.
diff --git a/openni2_camera/launch/camera_only.launch.py b/openni2_camera/launch/camera_only.launch.py
@@ -35,11 +35,19 @@
 import launch
 import launch_ros.actions
 import launch_ros.descriptions
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch.actions import DeclareLaunchArgument
 
 
 def generate_launch_description():
 
-    namespace = '/camera'
+    namespace_param_name = "namespace"
+    namespace = LaunchConfiguration(namespace_param_name)
+    namespace_launch_arg = DeclareLaunchArgument(namespace_param_name, default_value='camera')
+
+    tf_prefix_param_name = "tf_prefix"
+    tf_prefix = LaunchConfiguration(tf_prefix_param_name)
+    tf_prefix_launch_arg = DeclareLaunchArgument(tf_prefix_param_name, default_value='')
 
     container = launch_ros.actions.ComposableNodeContainer(
             name='container',
@@ -53,11 +61,30 @@ def generate_launch_description():
                     plugin='openni2_wrapper::OpenNI2Driver',
                     name='driver',
                     parameters=[{'depth_registration': True},
-                                {'use_device_time': True}],
+                                {'use_device_time': True},
+                                {'rgb_frame_id': [namespace,"_rgb_optical_frame"]},
+                                {'depth_frame_id': [namespace,"_depth_optical_frame"]},
+                                {'ir_frame_id': [namespace,"_ir_optical_frame"]},],
                     namespace=namespace,
                 ),
             ],
             output='screen',
     )
 
-    return launch.LaunchDescription([container])
+    tf_args = [
+      ["--frame-id", [tf_prefix,"/",namespace,"_link"], "--child-frame-id", [tf_prefix,"/",namespace,"_depth_frame"], "--y", "-0.02"],
+      ["--frame-id", [tf_prefix,"/",namespace,"_link"], "--child-frame-id", [tf_prefix,"/",namespace,"_rgb_frame"], "--y", "-0.045"],
+      ["--frame-id", [tf_prefix,"/",namespace,"_depth_frame"], "--child-frame-id", [tf_prefix,"/",namespace,"_depth_optical_frame"], "--roll", "-1.5707963267948966", "--yaw", "-1.5707963267948966"],
+      ["--frame-id", [tf_prefix,"/",namespace,"_rgb_frame"], "--child-frame-id", [tf_prefix,"/",namespace,"_rgb_optical_frame"], "--roll", "-1.5707963267948966", "--yaw", "-1.5707963267948966"],
+    ]
+
+    tf_nodes = [launch_ros.actions.Node(package='tf2_ros', executable='static_transform_publisher', output='screen', arguments=args) for args in tf_args]
+
+    return launch.LaunchDescription(
+      [
+        namespace_launch_arg,
+        tf_prefix_launch_arg,
+        container
+      ]
+      + tf_nodes
+    )