from launch.launch_description import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, FindExecutable, Command
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from launch.launch_description_sources import PythonLaunchDescriptionSource
import xacro
import os
from ament_index_python import get_package_share_directory



def generate_launch_description():

    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
    log_level = LaunchConfiguration('log_level', default='fatal')


    launch_args = []
    launch_args.append(
        DeclareLaunchArgument(
            name='use_sim_time',
            default_value=use_sim_time,
            description="If true, use simulated clock"
        )
    )
    launch_args.append(
        DeclareLaunchArgument(
            name='log_level',
            default_value=log_level,
            description="Log level of all nodes launched by this script"
        )
    )

    # JointTrajectory bridge for gripper (ROS2 -> IGN)
    joint_trajectory_controller_bridge = Node(package='ros_ign_bridge',
             executable='parameter_bridge',
             name='parameter_bridge_gripper_trajectory',
             output='screen',
             arguments=['/gripper_trajectory@trajectory_msgs/msg/JointTrajectory]ignition.msgs.JointTrajectory',
                        '--ros-args', '--log-level', log_level],
             parameters=[{'use_sim_time': use_sim_time}])

            # ros_ign_bridge (clock -> ROS 2)
    ros2_ign_clock_bridge = Node(
            package="ros_ign_bridge",
            executable="parameter_bridge",
            output="log",
            arguments=[
                "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
                "--ros-args",
                "--log-level",
                log_level,
            ],
            parameters=[{"use_sim_time": use_sim_time}],
        )

    launch_nodes = []
    #launch_nodes.append(joint_trajectory_controller_bridge)
    launch_nodes.append(ros2_ign_clock_bridge)

    return LaunchDescription(launch_nodes + launch_args)