import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess, TimerAction, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
import xacro

def generate_launch_description():
    camera_init = Node(
        package='openarm_camera',
        executable='camera_pub_node',
        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=8.0,
        actions=[stereo_hand_depth]
        ),
    ])
