2021-10-19 20:52:07 +04:00
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 )
2021-10-19 23:26:26 +04:00
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 )
2021-10-19 20:52:07 +04:00
robot_description = xacro . process_file ( xacro_file )
robot_description_content = robot_description . toxml ( )
#print(robot_description_content)
# Load controls
control = IncludeLaunchDescription (
PythonLaunchDescriptionSource (
PathJoinSubstitution ( [
2021-10-19 23:26:26 +04:00
FindPackageShare ( " rasms_moveit_config " ) ,
2021-10-19 20:52:07 +04:00
" launch " ,
2021-10-19 23:26:26 +04:00
" rasms_control.launch.py "
2021-10-19 20:52:07 +04:00
] )
) , 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 ( [
2021-10-19 23:26:26 +04:00
FindPackageShare ( " rasms_moveit_config " ) ,
2021-10-19 20:52:07 +04:00
" launch " ,
2021-10-19 23:26:26 +04:00
" rasms_simulation.launch.py "
2021-10-19 20:52:07 +04:00
] )
) ,
launch_arguments = [
( " robot_name " , LaunchConfiguration ( " robot_name " ) )
] ,
condition = IfCondition ( LaunchConfiguration ( " sim " ) )
)
# Move group
move_group = IncludeLaunchDescription (
PythonLaunchDescriptionSource (
PathJoinSubstitution ( [
2021-10-19 23:26:26 +04:00
FindPackageShare ( " rasms_moveit_config " ) ,
2021-10-19 20:52:07 +04:00
" launch " ,
2021-10-19 23:26:26 +04:00
" rasms_moveit_interface.launch.py "
2021-10-19 20:52:07 +04:00
] )
) ,
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 " ,
2021-10-19 23:26:26 +04:00
default_value = " rasms_description " ,
2021-10-19 20:52:07 +04:00
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 " ,
2021-10-19 23:26:26 +04:00
default_value = " rasms_moveit_config " ,
2021-10-19 20:52:07 +04:00
description = " Package that contains controller configurations. "
)
)
launch_args . append (
DeclareLaunchArgument (
name = " controller_configurations " ,
2021-10-19 23:26:26 +04:00
default_value = " config/rasms_controllers.yml " ,
2021-10-19 20:52:07 +04:00
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 " ,
2021-10-19 23:26:26 +04:00
default_value = " rasms_moveit_config " ,
2021-10-19 20:52:07 +04:00
description = " Package that contains MoveIt! controller configurations. "
)
)
launch_args . append (
DeclareLaunchArgument (
name = " moveit_controller_configurations " ,
2021-10-19 23:26:26 +04:00
default_value = " config/rasms_moveit_controllers.yml " ,
2021-10-19 20:52:07 +04:00
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 )
] )