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)