#!/usr/bin/env python3
#
# Launch depth image -> point cloud conversion for TurtleBot3 Gz Sim.
# Subscribes to the bridged L16 depth image and camera_info, and publishes
# a sensor_msgs/PointCloud2 that can be visualized in RViz.

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='true')

    declare_use_sim_time = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation clock if true')

    # Custom Python replacement for depth_image_proc/point_cloud_xyz.
    # It converts the bridged L16 depth image + camera_info into a PointCloud2.
    depth_to_cloud_node = Node(
        package='turtlebot3_gazebo',
        executable='depth_to_pointcloud.py',
        name='depth_to_pointcloud',
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen',
    )

    ld = LaunchDescription()
    ld.add_action(declare_use_sim_time)
    ld.add_action(depth_to_cloud_node)

    return ld
