108 lines
4.2 KiB
Python
108 lines
4.2 KiB
Python
import os
|
|
from launch_ros.actions import Node
|
|
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
|
from launch_ros.substitutions import FindPackageShare
|
|
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
|
|
from launch.conditions import IfCondition, UnlessCondition
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from ament_index_python.packages import get_package_share_directory
|
|
|
|
|
|
def generate_launch_description():
|
|
declared_arguments = []
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("sim_gazebo", default_value="false", description="Gazebo Simulation")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"rbs_robot_type",
|
|
description="Type of robot by name",
|
|
choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
|
default_value="ur5e",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("env_manager", default_value="false", description="Launch env_manager?")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("gazebo_gui", default_value="false", description="Launch env_manager?")
|
|
)
|
|
|
|
sim_gazebo = LaunchConfiguration("sim_gazebo")
|
|
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
|
|
env_manager_cond = LaunchConfiguration("env_manager")
|
|
gazebo_gui = LaunchConfiguration("gazebo_gui")
|
|
# FIXME: To args when we'll have different files
|
|
print(os.getenv("IGN_GAZEBO_RESOURCE_PATH").__str__())
|
|
world_config_file = PathJoinSubstitution(
|
|
[FindPackageShare("rbs_simulation"), "worlds", "asm2.sdf"]
|
|
)
|
|
# Gazebo nodes
|
|
|
|
gazebo_server = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[os.path.join(get_package_share_directory('ros_gz_sim'),
|
|
'launch', 'gz_sim.launch.py')]),
|
|
launch_arguments=[('ign_args', [' -r ',world_config_file, " -s"])],
|
|
condition=UnlessCondition(gazebo_gui))
|
|
|
|
gazebo = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[os.path.join(get_package_share_directory('ros_gz_sim'),
|
|
'launch', 'gz_sim.launch.py')]),
|
|
launch_arguments=[('ign_args', [' -r ',world_config_file])],
|
|
condition=IfCondition(gazebo_gui))
|
|
|
|
# Spawn robot
|
|
gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
|
|
arguments=[
|
|
'-name', rbs_robot_type,
|
|
'-x', '0.0',
|
|
'-z', '0.0',
|
|
'-y', '0.0',
|
|
'-topic', '/robot_description'],
|
|
output='screen',
|
|
condition=IfCondition(sim_gazebo))
|
|
env_manager = Node(package="env_manager",
|
|
executable="run_env_manager",
|
|
condition=IfCondition(env_manager_cond)
|
|
)
|
|
|
|
# Bridge
|
|
rgbd_bridge_out = Node(
|
|
package='ros_gz_bridge',
|
|
executable='parameter_bridge',
|
|
arguments=[
|
|
'/outer_rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
|
|
'/outer_rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
|
|
'/outer_rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image',
|
|
'/outer_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
|
|
],
|
|
output='screen',
|
|
condition=IfCondition(sim_gazebo)
|
|
)
|
|
|
|
rgbd_bridge_in = Node(
|
|
package='ros_gz_bridge',
|
|
executable='parameter_bridge',
|
|
arguments=[
|
|
'/inner_rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
|
|
'/inner_rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
|
|
'/inner_rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image',
|
|
'/inner_rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
|
|
],
|
|
output='screen',
|
|
)
|
|
|
|
|
|
|
|
nodes_to_start = [
|
|
gazebo,
|
|
gazebo_server,
|
|
gazebo_spawn_robot,
|
|
env_manager,
|
|
rgbd_bridge_out,
|
|
rgbd_bridge_in
|
|
]
|
|
return LaunchDescription(declared_arguments + nodes_to_start)
|