diff --git a/rosbot_xl_gazebo/launch/spawn.launch.py b/rosbot_xl_gazebo/launch/spawn.launch.py index 4259dc8..7ad6cec 100644 --- a/rosbot_xl_gazebo/launch/spawn.launch.py +++ b/rosbot_xl_gazebo/launch/spawn.launch.py @@ -35,6 +35,7 @@ def launch_gz_bridge(context: LaunchContext, *args, **kwargs): lidar_model = context.perform_substitution(LaunchConfiguration("lidar_model")) camera_model = context.perform_substitution(LaunchConfiguration("camera_model")) namespace = context.perform_substitution(LaunchConfiguration("namespace")) + actions = [] pointcloud_rpy = [ "1.57", @@ -55,105 +56,113 @@ def launch_gz_bridge(context: LaunchContext, *args, **kwargs): if lidar_model.startswith("slamtec_rplidar"): lidar_model = "slamtec_rplidar" - gz_lidar_remappings_file = PathJoinSubstitution( - [ - get_package_share_directory("rosbot_xl_gazebo"), - "config", - LaunchConfiguration( - "gz_lidar_remappings_file", - default=["gz_", lidar_model, "_remappings.yaml"], - ), - ] - ) - - namespaced_gz_lidar_remappings_file = ReplaceString( - source_file=gz_lidar_remappings_file, - replacements={"": namespace_ext}, - ) + if lidar_model != "None": + gz_lidar_remappings_file = PathJoinSubstitution( + [ + get_package_share_directory("rosbot_xl_gazebo"), + "config", + LaunchConfiguration( + "gz_lidar_remappings_file", + default=["gz_", lidar_model, "_remappings.yaml"], + ), + ] + ) - ign_lidar_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="ros_gz_lidar_bridge", - parameters=[{"config_file": namespaced_gz_lidar_remappings_file}], - remappings=[ - ("/tf", "tf"), - ("/tf_static", "tf_static"), - ], - output="screen", - namespace=namespace, - condition=LaunchConfigurationNotEquals(lidar_model, "None"), - ) + namespaced_gz_lidar_remappings_file = ReplaceString( + source_file=gz_lidar_remappings_file, + replacements={"": namespace_ext}, + ) - zed_model = None - if camera_model.startswith("stereolabs_zed"): - zed_model = camera_model.replace("stereolabs_", "") - camera_model = "stereolabs_zed" - depth_camera_child_tf = "rosbot_xl/base_link/camera_" + camera_model + "_depth" - depth_camera_parent_tf = "camera_center_optical_frame" - pointcloud_rpy = ["0", "0", "0"] + actions.append( + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="ros_gz_lidar_bridge", + parameters=[{"config_file": namespaced_gz_lidar_remappings_file}], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + output="screen", + namespace=namespace, + condition=LaunchConfigurationNotEquals(lidar_model, "None"), + ) + ) - gz_camera_remappings_file = PathJoinSubstitution( - [ - get_package_share_directory("rosbot_xl_gazebo"), - "config", - LaunchConfiguration( - "gz_camera_remappings_file", - default=["gz_", camera_model, "_remappings.yaml"], - ), - ] - ) + if camera_model != "None": + zed_model = None + if camera_model.startswith("stereolabs_zed"): + zed_model = camera_model.replace("stereolabs_", "") + camera_model = "stereolabs_zed" + depth_camera_child_tf = "rosbot_xl/base_link/camera_" + camera_model + "_depth" + depth_camera_parent_tf = "camera_center_optical_frame" + pointcloud_rpy = ["0", "0", "0"] - namespaced_gz_camera_remappings_file = ReplaceString( - source_file=gz_camera_remappings_file, - replacements={"": namespace_ext}, - ) + gz_camera_remappings_file = PathJoinSubstitution( + [ + get_package_share_directory("rosbot_xl_gazebo"), + "config", + LaunchConfiguration( + "gz_camera_remappings_file", + default=["gz_", camera_model, "_remappings.yaml"], + ), + ] + ) - if zed_model is not None: namespaced_gz_camera_remappings_file = ReplaceString( - source_file=namespaced_gz_camera_remappings_file, - replacements={"": zed_model}, + source_file=gz_camera_remappings_file, + replacements={"": namespace_ext}, ) - ign_camera_bridge = Node( - package="ros_gz_bridge", - executable="parameter_bridge", - name="ros_gz_camera_bridge", - parameters=[{"config_file": namespaced_gz_camera_remappings_file}], - remappings=[ - ("/tf", "tf"), - ("/tf_static", "tf_static"), - ], - output="screen", - namespace=namespace, - condition=LaunchConfigurationNotEquals(camera_model, "None"), - ) + if zed_model is not None: + namespaced_gz_camera_remappings_file = ReplaceString( + source_file=namespaced_gz_camera_remappings_file, + replacements={"": zed_model}, + ) - # The frame of the point cloud from ignition gazebo 6 isn't provided by . - # See https://github.com/gazebosim/gz-sensors/issues/239 - point_cloud_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="point_cloud_tf", - output="log", - arguments=[ - "0", - "0", - "0", - ] - + pointcloud_rpy - + [ - depth_camera_parent_tf, - depth_camera_child_tf, - ], - remappings=[ - ("/tf", "tf"), - ("/tf_static", "tf_static"), - ], - namespace=namespace, - ) + actions.append( + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="ros_gz_camera_bridge", + parameters=[{"config_file": namespaced_gz_camera_remappings_file}], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + output="screen", + namespace=namespace, + condition=LaunchConfigurationNotEquals(camera_model, "None"), + ) + ) + + # The frame of the point cloud from ignition gazebo 6 isn't provided by . + # See https://github.com/gazebosim/gz-sensors/issues/239 + actions.append( + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="point_cloud_tf", + output="log", + arguments=[ + "0", + "0", + "0", + ] + + pointcloud_rpy + + [ + depth_camera_parent_tf, + depth_camera_child_tf, + ], + remappings=[ + ("/tf", "tf"), + ("/tf_static", "tf_static"), + ], + namespace=namespace, + ) + ) - return [ign_lidar_bridge, ign_camera_bridge, point_cloud_tf] + return actions def generate_launch_description():