runtime/robossembler/launch/robossembler_bringup.launch.py

117 lines
3.6 KiB
Python

from launch.actions.declare_launch_argument import DeclareLaunchArgument
from launch.launch_description import LaunchDescription
from launch.actions import IncludeLaunchDescription, OpaqueFunction
from launch.conditions import IfCondition
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch.substitutions.launch_configuration import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python import get_package_share_directory
import xacro
import os
def generate_launch_description():
launch_args = []
launch_args.append(
DeclareLaunchArgument(
name="grip",
default_value="true",
description="On or OFF gripper for launch"
)
)
launch_args.append(
DeclareLaunchArgument(
name="sim",
default_value="true",
description="On or OFF simulation"
)
)
launch_args.append(
DeclareLaunchArgument(
name="robot_name",
default_value="rasmt",
description="Robot name"
)
)
launch_args.append(
DeclareLaunchArgument(
name="plansys2",
default_value="false",
description="enable plansys2"
)
)
# Load robot description
robot_description_content = Command(
[
FindExecutable(name="xacro"), " ",
PathJoinSubstitution(
[FindPackageShare("rasmt_support"), "urdf/rasmt.xacro"]
), " ",
"robot_name:=", LaunchConfiguration("robot_name"), " ",
"sim:=", LaunchConfiguration("sim"), " ",
"grip:=", LaunchConfiguration("grip")
]
)
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("rasmt_support"),
"launch",
"rasmt_control.launch.py"
])
), launch_arguments=[
("robot_description", robot_description_content),
("sim", LaunchConfiguration("sim"))
]
)
simulation = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("rasmt_support"),
"launch",
"rasmt_gazebo.launch.py"
])
), launch_arguments=[
("robot_name", LaunchConfiguration("robot_name"))
],
condition=IfCondition(LaunchConfiguration("sim"))
)
moveit = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("rasmt_moveit_config"),
"launch",
"rasmt_moveit.launch.py"
])
), launch_arguments=[
("robot_description", robot_description_content),
("sim",LaunchConfiguration("sim"))
]
)
task_planner_init = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
PathJoinSubstitution([
FindPackageShare("robossembler"),
"launch",
"task_planner.launch.py"
])
), launch_arguments=[
("robot_description", robot_description_content)
],
condition=IfCondition(LaunchConfiguration("plansys2"))
)
launch_nodes = []
launch_nodes.append(control)
launch_nodes.append(simulation)
launch_nodes.append(moveit)
launch_nodes.append(task_planner_init)
return LaunchDescription(launch_args + launch_nodes)