from launch import LaunchDescription
from launch.actions import RegisterEventHandler
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.event_handlers import OnProcessExit

from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():

    moveit_config = (
        MoveItConfigsBuilder("hexapod_robot", package_name = "hexapod_robot")

        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )        
        .planning_pipelines(pipelines = ["ompl"])
        .to_moveit_configs()
    )

    rviz_node = Node(
        package = "rviz2",
        executable = "rviz2",
        arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])],
        name = "rviz2",
        output = "screen",
        parameters = [moveit_config.planning_pipelines, moveit_config.robot_description_kinematics]
    )
    nodes = [
        rviz_node
    ]
            
    return LaunchDescription(nodes)