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


def generate_launch_description():
    return LaunchDescription([
        DeclareLaunchArgument(
            'frame',
            default_value='object',
            description='Name of the static frame'
        ),
        DeclareLaunchArgument(
            'position',
            default_value="0. 0. 1.",
            description='Position as x y z'
        ),
        DeclareLaunchArgument(
            'quaternion',
            default_value='0. 0. 0. 1.',
            description='Orientation as quaternion x y z w'),


        Node(
            package='rq_hardware',
            namespace='rq',
            executable='echo_service',
            name='hardware_interface'
        ),
        Node(
            package='rq_trajectory',
            namespace='rq',
            executable='echo_interference_service',
            name='trajectory'
        ),
        Node(
            package='rq_reconstruction',
            namespace='rq',
            executable='echo_service',
            name='reconstruction'
        ),
        Node(
            package='rq_ddetection',
            namespace='rq',
            executable='echo_service',
            name='defect_detection'
        ),
        Node(
            package='rq_controller',
            namespace='rq',
            executable='tf_static_broadcaster',
            name='object_broadcaster',
            parameters=[
                {'frame': LaunchConfiguration('frame')},
                {'position': LaunchConfiguration('position')},
                {'quaternion': LaunchConfiguration('quaternion')},
            ]
        ),
        
        ]
    )