publish tfs and set matching frame names (#135)

This PR ports the TFs from original
[`kinect_frames.launch`](https://github.com/ros-drivers/rgbd_launch/blob/noetic-devel/launch/kinect_frames.launch)
to the ROS 2 launch files and sets the image frame names appropriately.

openni2_camera/launch/camera_only.launch.py

@@ -35,11 +35,20 @@
 import launch
 import launch_ros.actions
 import launch_ros.descriptions
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 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 +62,24 @@ 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])
+    tfs = IncludeLaunchDescription(
+      PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), "tfs.launch.py"])),
+      launch_arguments={namespace_param_name: namespace, tf_prefix_param_name: tf_prefix}.items(),
+    )
+
+    return launch.LaunchDescription([
+      namespace_launch_arg,
+      tf_prefix_launch_arg,
+      container,
+      tfs,
+    ])

openni2_camera/launch/camera_with_cloud.launch.py

@@ -35,11 +35,20 @@
 import launch
 import launch_ros.actions
 import launch_ros.descriptions
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 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',
@@ -54,7 +63,10 @@ def generate_launch_description():
                     name='driver',
                     namespace=namespace,
                     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"]},],
                     remappings=[('depth/image', 'depth_registered/image_raw')],
                 ),
                 # Create XYZRGB point cloud
@@ -73,4 +85,14 @@ def generate_launch_description():
             output='screen',
     )
 
-    return launch.LaunchDescription([container])
+    tfs = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), "tfs.launch.py"])),
+        launch_arguments={namespace_param_name: namespace, tf_prefix_param_name: tf_prefix}.items(),
+    )
+
+    return launch.LaunchDescription([
+        namespace_launch_arg,
+        tf_prefix_launch_arg,
+        container,
+        tfs,
+    ])

openni2_camera/launch/camera_with_cloud_norgb.launch.py

@@ -35,11 +35,20 @@
 import launch
 import launch_ros.actions
 import launch_ros.descriptions
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 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',
@@ -54,7 +63,10 @@ def generate_launch_description():
                     name='driver',
                     namespace=namespace,
                     parameters=[{'depth_registration': False},
-                                {'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"]},],
                     remappings=[('depth/image', 'depth_registered/image_raw')],
                 ),
                 # Create XYZ point cloud
@@ -72,4 +84,14 @@ def generate_launch_description():
             output='screen',
     )
 
-    return launch.LaunchDescription([container])
+    tfs = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), "tfs.launch.py"])),
+        launch_arguments={namespace_param_name: namespace, tf_prefix_param_name: tf_prefix}.items(),
+    )
+
+    return launch.LaunchDescription([
+        namespace_launch_arg,
+        tf_prefix_launch_arg,
+        container,
+        tfs,
+    ])

openni2_camera/launch/tfs.launch.py

@@ -0,0 +1,34 @@
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    namespace_param_name = "namespace"
+    namespace = LaunchConfiguration(namespace_param_name)
+    namespace_launch_arg = DeclareLaunchArgument(namespace_param_name)
+
+    tf_prefix_param_name = "tf_prefix"
+    tf_prefix = LaunchConfiguration(tf_prefix_param_name)
+    tf_prefix_launch_arg = DeclareLaunchArgument(tf_prefix_param_name)
+
+    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 = [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] + tf_nodes)