From 512d8cdf3c2a1cc4df1ddfc1d674a20b7b391fe4 Mon Sep 17 00:00:00 2001 From: Christian Rauch Date: Mon, 13 Nov 2023 23:15:17 +0100 Subject: [PATCH] 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 | 28 +++++++++++++-- .../launch/camera_with_cloud.launch.py | 28 +++++++++++++-- .../launch/camera_with_cloud_norgb.launch.py | 28 +++++++++++++-- openni2_camera/launch/tfs.launch.py | 34 +++++++++++++++++++ 4 files changed, 109 insertions(+), 9 deletions(-) create mode 100644 openni2_camera/launch/tfs.launch.py diff --git a/openni2_camera/launch/camera_only.launch.py b/openni2_camera/launch/camera_only.launch.py index e67206e..1e4dc2d 100644 --- a/openni2_camera/launch/camera_only.launch.py +++ b/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, + ]) diff --git a/openni2_camera/launch/camera_with_cloud.launch.py b/openni2_camera/launch/camera_with_cloud.launch.py index 28b72c4..ac20c9b 100644 --- a/openni2_camera/launch/camera_with_cloud.launch.py +++ b/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, + ]) diff --git a/openni2_camera/launch/camera_with_cloud_norgb.launch.py b/openni2_camera/launch/camera_with_cloud_norgb.launch.py index d06bae6..a15e3cb 100644 --- a/openni2_camera/launch/camera_with_cloud_norgb.launch.py +++ b/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, + ]) diff --git a/openni2_camera/launch/tfs.launch.py b/openni2_camera/launch/tfs.launch.py new file mode 100644 index 0000000..ad006f3 --- /dev/null +++ b/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)