129 lines
4.5 KiB
Python
129 lines
4.5 KiB
Python
import os
|
|
import re
|
|
import subprocess
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument
|
|
from launch.actions import IncludeLaunchDescription
|
|
from launch.conditions import IfCondition
|
|
from launch.conditions import UnlessCondition
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch.substitutions import PathJoinSubstitution
|
|
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
|
|
|
def generate_launch_description():
|
|
|
|
resources_package = 'openarm_description_2'
|
|
|
|
# Make path to resources dir without last package_name fragment.
|
|
path_to_share_dir_clipped = ''.join(get_package_share_directory(resources_package).rsplit('/' + resources_package, 1))
|
|
|
|
# Gazebo hint for resources.
|
|
os.environ['GZ_SIM_RESOURCE_PATH'] = path_to_share_dir_clipped
|
|
|
|
# Ensure `SDF_PATH` is populated since `sdformat_urdf` uses this rather
|
|
|
|
# than `GZ_SIM_RESOURCE_PATH` to locate resources.
|
|
if "GZ_SIM_RESOURCE_PATH" in os.environ:
|
|
gz_sim_resource_path = os.environ["GZ_SIM_RESOURCE_PATH"]
|
|
|
|
if "SDF_PATH" in os.environ:
|
|
sdf_path = os.environ["SDF_PATH"]
|
|
os.environ["SDF_PATH"] = sdf_path + ":" + gz_sim_resource_path
|
|
else:
|
|
os.environ["SDF_PATH"] = gz_sim_resource_path
|
|
|
|
|
|
use_custom_world = LaunchConfiguration('use_custom_world')
|
|
use_custom_world_launch_arg = DeclareLaunchArgument('use_custom_world', default_value='true')
|
|
gazebo_world = LaunchConfiguration('gazebo_world')
|
|
gazebo_world_launch_arg = DeclareLaunchArgument('gazebo_world', default_value='empty.sdf')
|
|
|
|
# prepare custom world
|
|
world = os.getenv('GZ_SIM_WORLD', 'empty')
|
|
fly_world_path = resources_package + '/worlds/' + world + '.sdf'
|
|
gz_version = subprocess.getoutput("gz sim --versions")
|
|
gz_version_major = re.search(r'^\d{1}', gz_version).group()
|
|
launch_arguments=dict(gz_args = '-r ' + str(fly_world_path) + ' --verbose ', gz_version = gz_version_major).items()
|
|
|
|
# Gazebo Sim.
|
|
# by default the custom world is used, otherwise the gazebo world is used, which can be changed with the argument
|
|
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
|
|
gazebo = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'),
|
|
),
|
|
launch_arguments=launch_arguments if use_custom_world else dict(gz_args='-r ' + gazebo_world + ' --verbose').items(),
|
|
)
|
|
|
|
# Spawn
|
|
spawn = Node(
|
|
package='ros_gz_sim',
|
|
executable='create',
|
|
arguments=[
|
|
'-name', 'openarm',
|
|
'-x', '1.2',
|
|
'-z', '2.3',
|
|
'-Y', '3.4',
|
|
'-topic', '/robot_description',
|
|
],
|
|
output='screen',
|
|
)
|
|
|
|
use_sim_time = LaunchConfiguration('use_sim_time')
|
|
use_sim_time_launch_arg = DeclareLaunchArgument('use_sim_time', default_value='true')
|
|
use_rviz = LaunchConfiguration('use_rviz')
|
|
use_rviz_arg = DeclareLaunchArgument("use_rviz", default_value='true')
|
|
|
|
robot_state_publisher = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource([
|
|
PathJoinSubstitution([
|
|
FindPackageShare(resources_package),
|
|
'launch',
|
|
'description.launch.py',
|
|
]),
|
|
]),
|
|
condition=UnlessCondition(use_rviz), # rviz launch includes rsp.
|
|
launch_arguments=dict(use_sim_time=use_sim_time).items(),
|
|
)
|
|
|
|
rviz = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource([
|
|
PathJoinSubstitution([
|
|
FindPackageShare(resources_package),
|
|
'launch',
|
|
'display.launch.py',
|
|
]),
|
|
]),
|
|
condition=IfCondition(use_rviz),
|
|
launch_arguments=dict(use_sim_time=use_sim_time).items(),
|
|
)
|
|
|
|
gz_bridge = Node(
|
|
package='ros_gz_bridge',
|
|
executable='parameter_bridge',
|
|
arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
|
|
output='screen',
|
|
parameters=[{
|
|
'use_sim_time': use_sim_time,
|
|
}],
|
|
)
|
|
|
|
return LaunchDescription([
|
|
use_sim_time_launch_arg,
|
|
use_rviz_arg,
|
|
use_custom_world_launch_arg,
|
|
gazebo_world_launch_arg,
|
|
robot_state_publisher,
|
|
rviz,
|
|
gazebo,
|
|
spawn,
|
|
gz_bridge,
|
|
])
|