from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import ThisLaunchFileDir
from launch_ros.actions import Node
from launch.substitutions import Command, FindExecutable, LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    go2_xacro_file = os.path.join(
        get_package_share_directory("go2_description"), "xacro", "robot_virtual_arm.xacro"
    )
    robot_description = Command(
        [FindExecutable(name="xacro"), " ", go2_xacro_file, " DEBUG:=", 'false']
    )

    return LaunchDescription([
        # launch the pointcloud to laser scan converter
        Node(
            package='go2py_node',
            executable='bridge',
            name='go2py_bridge'
        ),
        Node(
                package="robot_state_publisher",
                executable="robot_state_publisher",
                name="robot_state_publisher",
                output="screen",
                parameters=[{"robot_description": robot_description}],
                remappings=[
                    ("/joint_states", "/go2/joint_states"),
                    ("/robot_description", "/go2/robot_description"),
                ],
            ),
        Node(
                package="tf2_ros",
                executable="static_transform_publisher",
                arguments=[
                    "0.15",
                    "0",
                    "0.15",
                    "0",
                    "0",
                    "0.707107",
                    "0.707107",
                    "/trunk",
                    "/go2/hesai_lidar",
                ],
                name="static_tf_pub_trunk_to_lidar",
            ),
    ])