|
| 1 | +# Author: Marq Rasmussen |
| 2 | + |
| 3 | +from launch import LaunchDescription, LaunchContext |
| 4 | +from launch.actions import ( |
| 5 | + DeclareLaunchArgument, |
| 6 | + IncludeLaunchDescription, |
| 7 | + OpaqueFunction, |
| 8 | +) |
| 9 | +from launch.launch_description_sources import PythonLaunchDescriptionSource |
| 10 | +from launch.conditions import IfCondition |
| 11 | +from launch.substitutions import ( |
| 12 | + Command, |
| 13 | + FindExecutable, |
| 14 | + LaunchConfiguration, |
| 15 | + PathJoinSubstitution, |
| 16 | +) |
| 17 | +from launch_ros.actions import Node |
| 18 | +from launch_ros.substitutions import FindPackageShare |
| 19 | +from ament_index_python.packages import get_package_share_directory |
| 20 | + |
| 21 | + |
| 22 | +def launch_gz(context: LaunchContext): |
| 23 | + gz_world_file = ( |
| 24 | + get_package_share_directory("realsense2_gz_description") + "/world/example.sdf" |
| 25 | + ) |
| 26 | + # -r is to run the simulation on start |
| 27 | + # -v is the verbose level |
| 28 | + # 0: No output, 1: Error, 2: Error and warning, 3: Error, warning, and info, 4: Error, warning, info, and debug. |
| 29 | + sim_options = "-r -v 3" |
| 30 | + if LaunchConfiguration("headless").perform(context) == "true": |
| 31 | + sim_options += " -s" # -s is to only run the server (headless mode). |
| 32 | + gz_launch_description = IncludeLaunchDescription( |
| 33 | + PythonLaunchDescriptionSource( |
| 34 | + [FindPackageShare("ros_gz_sim"), "/launch/gz_sim.launch.py"] |
| 35 | + ), |
| 36 | + launch_arguments=[("gz_args", [f"{sim_options} {gz_world_file}"])], |
| 37 | + ) |
| 38 | + return [gz_launch_description] |
| 39 | + |
| 40 | + |
| 41 | +def generate_launch_description(): |
| 42 | + declared_arguments = [] |
| 43 | + declared_arguments.append( |
| 44 | + DeclareLaunchArgument("rviz", default_value="true", description="Launch RViz?") |
| 45 | + ) |
| 46 | + declared_arguments.append( |
| 47 | + DeclareLaunchArgument( |
| 48 | + "headless", default_value="true", description="Launch Gazebo headless?" |
| 49 | + ) |
| 50 | + ) |
| 51 | + |
| 52 | + # Initialize Arguments |
| 53 | + launch_rviz = LaunchConfiguration("rviz") |
| 54 | + |
| 55 | + rviz_config_file = PathJoinSubstitution( |
| 56 | + [FindPackageShare("realsense2_gz_description"), "rviz", "example.rviz"] |
| 57 | + ) |
| 58 | + |
| 59 | + robot_description_content = Command( |
| 60 | + [ |
| 61 | + PathJoinSubstitution([FindExecutable(name="xacro")]), |
| 62 | + " ", |
| 63 | + PathJoinSubstitution( |
| 64 | + [ |
| 65 | + FindPackageShare("realsense2_gz_description"), |
| 66 | + "urdf", |
| 67 | + "example_d415_gazebo.urdf.xacro", |
| 68 | + ] |
| 69 | + ), |
| 70 | + " ", |
| 71 | + "camera_name:=gz_camera", |
| 72 | + " ", |
| 73 | + ] |
| 74 | + ) |
| 75 | + |
| 76 | + robot_state_publisher_node = Node( |
| 77 | + package="robot_state_publisher", |
| 78 | + executable="robot_state_publisher", |
| 79 | + output="both", |
| 80 | + parameters=[ |
| 81 | + { |
| 82 | + "use_sim_time": True, |
| 83 | + "robot_description": robot_description_content, |
| 84 | + } |
| 85 | + ], |
| 86 | + ) |
| 87 | + |
| 88 | + rviz_node = Node( |
| 89 | + package="rviz2", |
| 90 | + executable="rviz2", |
| 91 | + name="rviz2", |
| 92 | + output="log", |
| 93 | + parameters=[{"use_sim_time": True}], |
| 94 | + arguments=["-d", rviz_config_file], |
| 95 | + condition=IfCondition(launch_rviz), |
| 96 | + ) |
| 97 | + |
| 98 | + ignition_spawn_entity = Node( |
| 99 | + package="ros_gz_sim", |
| 100 | + executable="create", |
| 101 | + output="screen", |
| 102 | + arguments=[ |
| 103 | + "-string", |
| 104 | + robot_description_content, |
| 105 | + "-name", |
| 106 | + "realsense_camera", |
| 107 | + "-allow_renaming", |
| 108 | + "true", |
| 109 | + "-x", |
| 110 | + "0.0", |
| 111 | + "-y", |
| 112 | + "0.0", |
| 113 | + "-z", |
| 114 | + "0.0", |
| 115 | + "-R", |
| 116 | + "0.0", |
| 117 | + "-P", |
| 118 | + "0.0", |
| 119 | + "-Y", |
| 120 | + "0.0", |
| 121 | + ], |
| 122 | + ) |
| 123 | + |
| 124 | + # Bridge the camera data to ROS and match the default topics that the real camera would publish |
| 125 | + # Note the gz_topic_name comes from _realsense_model.gazebo.xacro defaults which defaults to `camera` here. |
| 126 | + gazebo_bridge = Node( |
| 127 | + package="ros_gz_bridge", |
| 128 | + executable="parameter_bridge", |
| 129 | + parameters=[{"use_sim_time": True}], |
| 130 | + arguments=[ |
| 131 | + "/camera/image@sensor_msgs/msg/Image[ignition.msgs.Image", |
| 132 | + "/camera/depth_image@sensor_msgs/msg/Image[ignition.msgs.Image", |
| 133 | + "/camera/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked", |
| 134 | + "/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo", |
| 135 | + "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", |
| 136 | + ], |
| 137 | + remappings=[ |
| 138 | + ( |
| 139 | + "/camera/image", |
| 140 | + "/camera/camera/color/image_raw", |
| 141 | + ), |
| 142 | + ( |
| 143 | + "/camera/depth_image", |
| 144 | + "/camera/camera/depth_registered/image_rect", |
| 145 | + ), |
| 146 | + ( |
| 147 | + "/camera/points", |
| 148 | + "/camera/camera/depth/color/points", |
| 149 | + ), |
| 150 | + ( |
| 151 | + "/camera/camera_info", |
| 152 | + "/camera/camera/color/camera_info", |
| 153 | + ), |
| 154 | + ( |
| 155 | + "/camera/camera_info", |
| 156 | + "/camera/camera/depth_registered/camera_info", |
| 157 | + ), |
| 158 | + ], |
| 159 | + output="screen", |
| 160 | + ) |
| 161 | + |
| 162 | + nodes_to_start = [ |
| 163 | + robot_state_publisher_node, |
| 164 | + rviz_node, |
| 165 | + OpaqueFunction(function=launch_gz), |
| 166 | + ignition_spawn_entity, |
| 167 | + gazebo_bridge, |
| 168 | + ] |
| 169 | + |
| 170 | + return LaunchDescription(declared_arguments + nodes_to_start) |
0 commit comments