117 lines
3.6 KiB
Python
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)
|