import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch_ros.actions import Node, SetParameter
from launch.launch_description_sources import PythonLaunchDescriptionSource
from moveit_configs_utils import MoveItConfigsBuilder
from xacro import process as xacro_process

def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config").to_moveit_configs()

    return LaunchDescription([
        SetParameter(name="use_sim_time", value=True),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
            launch_arguments={'gz_args': '-v 4 -r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
        ),

        Node(
            package='ros_gz_bridge',
            executable='parameter_bridge',
            arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
            output='both',
        ),

        #Node(
        #    package='ros_gz_bridge',
        #    executable='parameter_bridge',
        #    arguments=["/world/default/dynamic_pose/info@tf2_msgs/msg/TFMessage]ignition.msgs.Pose_V"],
        #    output='both',
        #    remappings=[
        #        ('/world/default/dynamic_pose/info', '/tf'),
        #    ]
        #),

        Node(
            package='ros_actor_action_server',
            executable='ros_actor_action_server'
        ),

        #Node(
        #    package='ros_gz_sim',
        #    executable='create',
        #    arguments=['-world', 'default', '-string', xacro_process(os.path.join(get_package_share_directory('iisy_config'), 'config', 'iisy_xacro.urdf'))],
        #    output='screen'
        #),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                str(moveit_config.package_path / "launch/demo.launch.py")
            ),
        ),
    ])