import os
from launch import LaunchDescription
from launch.actions import TimerAction
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    params_file = os.path.join(
        get_package_share_directory('openarm_camera'),
        'config',
        'camera_pub.params.yaml'
    )

    camera_init = Node(
        package='openarm_camera',
        executable='camera_pub_node',
        parameters=[params_file],
        output='screen'
    )
    camera_cal_init = Node(
        package='openarm_camera',
        executable='camera_node',
        output='screen'
    )
    stereo_hand_depth = Node(
        package='openarm_camera',
        executable='stereo_hand_depth_node',
        output='screen'
    )

    static_tf_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='camera_static_tf',
        arguments=[
            '0.0', '0.0', '0.0',   # x y z
            '0', '0', '0',         # roll pitch yaw
            'openarm_body_link0',
            'camera_link'
        ]
    )
    return LaunchDescription([
        camera_init,
        static_tf_node,
        # TimerAction(
        # period=4.0,
        # actions=[camera_cal_init]
        # ),
        # TimerAction(
        # period=4.0,
        # actions=[stereo_hand_depth]
        # ),
    ])
