"""
realsense.launch.py — Intel RealSense D435i driver (standalone)

Launches realsense2_camera_node with Jetson Orin Nano Super power-budget settings:
  - 640×480 @ 15fps (depth + RGB) — saves ~0.4W vs 30fps
  - IMU enabled with linear interpolation (unified /camera/imu topic)
  - Depth aligned to color frame
  - Point cloud disabled (expensive on Maxwell GPU)

Published topics:
  /camera/color/image_raw          sensor_msgs/Image         15 Hz
  /camera/color/camera_info        sensor_msgs/CameraInfo    15 Hz
  /camera/depth/image_rect_raw     sensor_msgs/Image         15 Hz
  /camera/depth/camera_info        sensor_msgs/CameraInfo    15 Hz
  /camera/aligned_depth_to_color/image_raw  sensor_msgs/Image  15 Hz
  /camera/imu                      sensor_msgs/Imu           ~200 Hz

Verify:
  ros2 topic list | grep camera
  ros2 topic hz /camera/color/image_raw
  ros2 topic hz /camera/imu
"""

import os
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # Allow overriding camera serial number at launch time (useful with multiple cameras)
    serial_no_arg = DeclareLaunchArgument(
        'serial_no',
        default_value="''",
        description='RealSense device serial number (empty = first found)',
    )

    realsense_share = get_package_share_directory('realsense2_camera')

    realsense_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(realsense_share, 'launch', 'rs_launch.py')
        ),
        launch_arguments={
            # Camera identity
            'serial_no':              LaunchConfiguration('serial_no'),
            'camera_name':            'camera',
            'camera_namespace':       'camera',

            # Depth stream — 640×480 @ 15fps saves power on Nano
            'depth_module.profile':   '640x480x15',
            'enable_depth':           'true',

            # RGB stream — matched resolution/fps to depth
            'rgb_camera.profile':     '640x480x15',
            'enable_color':           'true',

            # IR streams — disabled (not needed for RTAB-Map RGB-D mode)
            'enable_infra1':          'false',
            'enable_infra2':          'false',

            # IMU — enable both sensors, publish unified topic
            'enable_gyro':            'true',
            'enable_accel':           'true',
            # 2 = linear_interpolation: aligns accel+gyro timestamps
            'unite_imu_method':       '2',

            # Align depth to color frame (required for rtabmap_ros RGB-D input)
            'align_depth.enable':     'true',

            # Point cloud — disabled, too expensive for Maxwell GPU during SLAM
            'pointcloud.enable':      'false',

            # TF — publish camera→IMU extrinsics
            'publish_tf':             'true',
            'tf_publish_rate':        '0.0',   # static only, no redundant updates
        }.items(),
    )

    return LaunchDescription([
        serial_no_arg,
        realsense_launch,
    ])