148 lines
5.2 KiB
Python
148 lines
5.2 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
|
|
import xacro
|
|
import os
|
|
from ament_index_python import get_package_share_directory
|
|
|
|
|
|
def launch_setup(context, *args, **kwargs):
|
|
|
|
# Evaluate frequently used variables
|
|
model = LaunchConfiguration("model").perform(context)
|
|
xacro_file = os.path.join(get_package_share_directory("rasms_description"),"urdf/","rasms_description.urdf.xacro")
|
|
assert os.path.exists(xacro_file), "The xacro file of rasms_description doesnt exist"+str(xacro_file)
|
|
robot_description = xacro.process_file(xacro_file)
|
|
robot_description_content = robot_description.toxml()
|
|
#print(robot_description_content)
|
|
|
|
# Load controls
|
|
control = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
PathJoinSubstitution([
|
|
FindPackageShare("rasms_moveit_config"),
|
|
"launch",
|
|
"rasms_control.launch.py"
|
|
])
|
|
), launch_arguments=[
|
|
("robot_description", robot_description_content),
|
|
("controller_configurations_package", LaunchConfiguration("controller_configurations_package")),
|
|
("controller_configurations", LaunchConfiguration("controller_configurations")),
|
|
("controller", LaunchConfiguration("controller")),
|
|
("sim", LaunchConfiguration("sim"))
|
|
]
|
|
)
|
|
|
|
# Gazebo simulation
|
|
simulation = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
PathJoinSubstitution([
|
|
FindPackageShare("rasms_moveit_config"),
|
|
"launch",
|
|
"rasms_simulation.launch.py"
|
|
])
|
|
),
|
|
launch_arguments=[
|
|
("robot_name", LaunchConfiguration("robot_name"))
|
|
],
|
|
condition=IfCondition(LaunchConfiguration("sim"))
|
|
)
|
|
|
|
# Move group
|
|
move_group = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
PathJoinSubstitution([
|
|
FindPackageShare("rasms_moveit_config"),
|
|
"launch",
|
|
"rasms_moveit_interface.launch.py"
|
|
])
|
|
),
|
|
launch_arguments=[
|
|
("robot_description", robot_description_content),
|
|
("moveit_controller_configurations_package", LaunchConfiguration("moveit_controller_configurations_package")),
|
|
("moveit_controller_configurations", LaunchConfiguration("moveit_controller_configurations")),
|
|
("model", LaunchConfiguration("model")),
|
|
("sim", LaunchConfiguration("sim"))
|
|
]
|
|
)
|
|
|
|
return [
|
|
simulation,
|
|
control,
|
|
move_group
|
|
]
|
|
|
|
|
|
def generate_launch_description():
|
|
|
|
# Launch arguments
|
|
launch_args = []
|
|
|
|
launch_args.append(DeclareLaunchArgument(
|
|
name="model",
|
|
default_value="rasms_description",
|
|
description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14."
|
|
))
|
|
|
|
launch_args.append(DeclareLaunchArgument(
|
|
name="robot_name",
|
|
default_value="rasms",
|
|
description="Set robot name."
|
|
))
|
|
|
|
launch_args.append(DeclareLaunchArgument(
|
|
name="sim",
|
|
default_value="true",
|
|
description="Launch robot in simulation or on real setup."
|
|
))
|
|
|
|
launch_args.append(
|
|
DeclareLaunchArgument(
|
|
name="controller_configurations_package",
|
|
default_value="rasms_moveit_config",
|
|
description="Package that contains controller configurations."
|
|
)
|
|
)
|
|
|
|
launch_args.append(
|
|
DeclareLaunchArgument(
|
|
name="controller_configurations",
|
|
default_value="config/rasms_controllers.yml",
|
|
description="Relative path to controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name."
|
|
)
|
|
)
|
|
|
|
launch_args.append(
|
|
DeclareLaunchArgument(
|
|
name="controller",
|
|
default_value="position_trajectory_controller",
|
|
description="Robot controller."
|
|
)
|
|
)
|
|
|
|
launch_args.append(
|
|
DeclareLaunchArgument(
|
|
name="moveit_controller_configurations_package",
|
|
default_value="rasms_moveit_config",
|
|
description="Package that contains MoveIt! controller configurations."
|
|
)
|
|
)
|
|
|
|
launch_args.append(
|
|
DeclareLaunchArgument(
|
|
name="moveit_controller_configurations",
|
|
default_value="config/rasms_moveit_controllers.yml",
|
|
description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name. This file lists controllers that are loaded through the controller_configurations file."
|
|
)
|
|
)
|
|
|
|
return LaunchDescription(
|
|
launch_args + [
|
|
OpaqueFunction(function=launch_setup)
|
|
])
|