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

def generate_launch_description():
    urdf_package_path = get_package_share_directory('fishbot_description')
    default_xacro_path = os.path.join(urdf_package_path,'urdf','fishbot','fishbot.urdf.xacro')
    default_gazebo_world_path = os.path.join(urdf_package_path,'world','custom_room.world')
    default_rviz_config_path = os.path.join(urdf_package_path,'config','display_robot_model.rviz')
    action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument(
        name='model',default_value=str(default_xacro_path),description="加载的模型文件路径"
    )
    
    substitutions_command_result1 = launch.substitutions.Command(['xacro ',launch.substitutions.LaunchConfiguration('model'),' robotname:=robot1 prefix:=fishbot1'])
    robot_description_value1 = launch_ros.parameter_descriptions.ParameterValue(substitutions_command_result1,value_type=str)
    substitutions_command_result2 = launch.substitutions.Command(['xacro ',launch.substitutions.LaunchConfiguration('model'),' robotname:=robot2 prefix:=fishbot2'])
    robot_description_value2 = launch_ros.parameter_descriptions.ParameterValue(substitutions_command_result2,value_type=str)


    action_robot_state_publisher1 = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        namespace='robot1',
        parameters=[{'robot_description':robot_description_value1}]
    )
    action_robot_state_publisher2 = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        namespace='robot2',
        parameters=[{'robot_description':robot_description_value2}]
    )


    #这是启动另外一个launch文件的方法，在这启动gazebo自带的launch文件
    action_launch_gazebo = launch.actions.IncludeLaunchDescription(
        launch.launch_description_sources.PythonLaunchDescriptionSource(
            [get_package_share_directory('gazebo_ros'),'/launch','/gazebo.launch.py']
        ),
        launch_arguments=[('world',default_gazebo_world_path),('verbose','true')]
    )

    action_spawn_entity1 = launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        namespace="/robot1",
        arguments=['-topic','/robot1/robot_description','-entity','robot1',
        '-x', '1.0',
        '-y', '0.0',
        '-z', '1.0',
        '-Y', '1.57']
    )
    action_spawn_entity2 = launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        namespace="/robot2",
        arguments=['-topic','/robot2/robot_description','-entity','robot2',
        '-x', '0.0',
        '-y', '0.0',
        '-z', '1.0',
        '-Y', '1.57']
    )

    joint_state_broadcaster_spawner1 = launch_ros.actions.Node(
        package="controller_manager",
        executable="spawner",
        name="robot1_joint",
        namespace="robot1",
        arguments=[
            "fishbot_joint_state_broadcaster",
            "--controller-manager",
            "controller_manager"
        ],
        output="screen",
    )
    joint_state_broadcaster_spawner2 = launch_ros.actions.Node(
        package="controller_manager",
        executable="spawner",
        name="robot2_joint",
        namespace="robot2",
        arguments=[
            "fishbot_joint_state_broadcaster",
            "--controller-manager",
            "controller_manager"
        ],
        output="screen",
    )

    diff_drive_controller_spawner1 = launch_ros.actions.Node(
        package="controller_manager",
        executable="spawner",
        name="robot1_diff",
        namespace="robot1",
        arguments=[
            "fishbot_diff_drive_controller",
            "--controller-manager",
            "controller_manager"
        ],
        output="screen",
    )
    diff_drive_controller_spawner2 = launch_ros.actions.Node(
        package="controller_manager",
        executable="spawner",
        name="robot2_diff",
        namespace="robot2",
        arguments=[
            "fishbot_diff_drive_controller",
            "--controller-manager",
            "controller_manager"
        ],
        output="screen",
    )

    return launch.LaunchDescription([
                action_declare_arg_mode_path,
                action_launch_gazebo,

                TimerAction(
                    period=2.0,
                    actions=[
                        action_robot_state_publisher2,
                        action_spawn_entity2,
                    ]
                ),


                TimerAction(
                    period=4.0,
                    actions=[
                        action_robot_state_publisher1,
                        action_spawn_entity1,
                    ]
                ),

        # launch.actions.RegisterEventHandler(
        #     event_handler=launch.event_handlers.OnProcessExit(
        #         target_action=action_spawn_entity1,
        #         on_exit=joint_state_broadcaster_spawner1
        #     )
        # ),
        # launch.actions.RegisterEventHandler(
        #     event_handler=launch.event_handlers.OnProcessExit(
        #         target_action=action_spawn_entity2,
        #         on_exit=joint_state_broadcaster_spawner2
        #     )
        # ),
        # launch.actions.RegisterEventHandler(
        #     event_handler=launch.event_handlers.OnProcessExit(
        #         target_action=joint_state_broadcaster_spawner1,
        #         on_exit=diff_drive_controller_spawner1
        #     )
        # ),
        # launch.actions.RegisterEventHandler(
        #     event_handler=launch.event_handlers.OnProcessExit(
        #         target_action=joint_state_broadcaster_spawner2,
        #         on_exit=diff_drive_controller_spawner2
        #     )
        # ),
    ])