127 lines
5 KiB
Python
127 lines
5 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=["rbs_arm" ,"ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
|
default_value="rbs_arm",)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("gazebo_gui",
|
|
default_value="true",
|
|
description="Launch gazebo_gui?")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("debugger",
|
|
default_value="false",
|
|
description="launch Gazebo with debugger?")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("vision_bridge",
|
|
default_value="false",
|
|
description="Create bridge for rgbd cameras")
|
|
)
|
|
|
|
sim_gazebo = LaunchConfiguration("sim_gazebo")
|
|
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
|
|
gazebo_gui = LaunchConfiguration("gazebo_gui")
|
|
debugger = LaunchConfiguration("debugger")
|
|
vision_bridge = LaunchConfiguration("vision_bridge")
|
|
|
|
# FIXME: To args when we'll have different files
|
|
# TODO: Use global variable to find world file in robossembler_db
|
|
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={
|
|
'gz_args': [' -r ',world_config_file, " -s"],
|
|
"debugger": debugger,
|
|
}.items(),
|
|
condition=UnlessCondition(gazebo_gui))
|
|
|
|
gazebo = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[os.path.join(get_package_share_directory('ros_gz_sim'),
|
|
'launch', 'gz_sim.launch.py')]),
|
|
launch_arguments={
|
|
'gz_args': [' -r ',world_config_file],
|
|
"debugger": debugger,
|
|
}.items(),
|
|
condition=IfCondition(gazebo_gui))
|
|
|
|
# Spawn robot
|
|
gazebo_spawn_robot = Node(
|
|
package='ros_gz_sim',
|
|
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))
|
|
|
|
# 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) and IfCondition(vision_bridge))
|
|
|
|
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',
|
|
condition=IfCondition(sim_gazebo) and IfCondition(vision_bridge))
|
|
|
|
clock_bridge = Node(
|
|
package='ros_gz_bridge',
|
|
executable='parameter_bridge',
|
|
arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'],
|
|
output='screen',
|
|
condition=IfCondition(sim_gazebo))
|
|
|
|
nodes_to_start = [
|
|
gazebo,
|
|
gazebo_server,
|
|
gazebo_spawn_robot,
|
|
rgbd_bridge_out,
|
|
rgbd_bridge_in,
|
|
clock_bridge,
|
|
]
|
|
return LaunchDescription(declared_arguments + nodes_to_start)
|