import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import Command
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


package_name = "gr_vision60"


def generate_launch_description():
    # log format
    os.environ["RCUTILS_CONSOLE_OUTPUT_FORMAT"] = (
        "[{time}] [{name}] [{severity}] {message}"
    )

    pkg_share_path = get_package_share_directory(package_name)
    urdf_file = os.path.join(pkg_share_path, "urdf", "vision60_v4_composite.urdf.xacro")
    robot_desc = ParameterValue(
        Command(["xacro", " ", str(urdf_file)]),
        value_type=str,
    )
    return LaunchDescription(
        [
            Node(
                package="robot_state_publisher",
                executable="robot_state_publisher",
                name="vision60_state_publisher",
                output="screen",
                emulate_tty=True,
                parameters=[{"robot_description": robot_desc}],
            )
        ]
    )