2024-04-18 13:29:36 +00:00
|
|
|
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("gazebo_gui",
|
2024-07-04 11:38:08 +00:00
|
|
|
default_value="false",
|
2024-10-08 16:41:40 +03:00
|
|
|
description="Launch gazebo_gui?")
|
2024-04-18 13:29:36 +00:00
|
|
|
)
|
|
|
|
declared_arguments.append(
|
|
|
|
DeclareLaunchArgument("debugger",
|
|
|
|
default_value="false",
|
|
|
|
description="launch Gazebo with debugger?")
|
|
|
|
)
|
|
|
|
declared_arguments.append(
|
2024-07-04 11:38:08 +00:00
|
|
|
DeclareLaunchArgument("rgbd_camera",
|
|
|
|
default_value="true",
|
|
|
|
description="Camera are used?")
|
2024-04-18 13:29:36 +00:00
|
|
|
)
|
|
|
|
|
|
|
|
sim_gazebo = LaunchConfiguration("sim_gazebo")
|
|
|
|
gazebo_gui = LaunchConfiguration("gazebo_gui")
|
|
|
|
debugger = LaunchConfiguration("debugger")
|
2024-07-04 11:38:08 +00:00
|
|
|
rgbd_camera = LaunchConfiguration("rgbd_camera")
|
2024-04-18 13:29:36 +00:00
|
|
|
gazebo_world_filename = LaunchConfiguration("gazebo_world_filename")
|
|
|
|
|
|
|
|
world_config_file = PathJoinSubstitution(
|
|
|
|
[FindPackageShare("rbs_simulation"), "worlds", gazebo_world_filename]
|
|
|
|
)
|
|
|
|
gazebo_server = IncludeLaunchDescription(
|
|
|
|
PythonLaunchDescriptionSource(
|
|
|
|
[os.path.join(get_package_share_directory('ros_gz_sim'),
|
|
|
|
'launch', 'gz_sim.launch.py')]),
|
|
|
|
launch_arguments={
|
2024-07-04 11:38:08 +00:00
|
|
|
'gz_args': [' -s ', '-r ', world_config_file],
|
2024-04-18 13:29:36 +00:00
|
|
|
"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={
|
2024-07-04 11:38:08 +00:00
|
|
|
'gz_args': [' -r ', world_config_file],
|
2024-04-18 13:29:36 +00:00
|
|
|
"debugger": debugger,
|
|
|
|
}.items(),
|
|
|
|
condition=IfCondition(gazebo_gui))
|
|
|
|
|
2024-07-04 11:38:08 +00:00
|
|
|
rgbd_sensor_bridge = Node(
|
|
|
|
package="ros_gz_bridge",
|
|
|
|
executable="parameter_bridge",
|
|
|
|
arguments=[
|
|
|
|
'/rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image',
|
|
|
|
'/rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo',
|
|
|
|
'/rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image',
|
|
|
|
'/rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked'
|
|
|
|
],
|
|
|
|
output='screen',
|
|
|
|
condition=IfCondition(rgbd_camera)
|
|
|
|
)
|
2024-04-18 13:29:36 +00:00
|
|
|
|
|
|
|
|
|
|
|
clock_bridge = Node(
|
|
|
|
package='ros_gz_bridge',
|
|
|
|
executable='parameter_bridge',
|
2024-07-04 11:38:08 +00:00
|
|
|
arguments=[
|
|
|
|
'/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'
|
|
|
|
],
|
2024-04-18 13:29:36 +00:00
|
|
|
output='screen',
|
|
|
|
condition=IfCondition(sim_gazebo))
|
|
|
|
|
|
|
|
nodes_to_start = [
|
|
|
|
gazebo,
|
|
|
|
gazebo_server,
|
|
|
|
clock_bridge,
|
2024-07-04 11:38:08 +00:00
|
|
|
rgbd_sensor_bridge,
|
2024-04-18 13:29:36 +00:00
|
|
|
]
|
|
|
|
return LaunchDescription(declared_arguments + nodes_to_start)
|