Skip to content

Commit

Permalink
None sensor patch
Browse files Browse the repository at this point in the history
  • Loading branch information
@rafal-gorecki
rafal-gorecki committed Mar 1, 2024
1 parent f55bdd5 commit f49120d
Showing 1 changed file with 98 additions and 89 deletions.
187 changes: 98 additions & 89 deletions rosbot_xl_gazebo/launch/spawn.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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",
Expand All @@ -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={"<robot_namespace>": 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={"<robot_namespace>": 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={"<robot_namespace>": 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>": zed_model},
source_file=gz_camera_remappings_file,
replacements={"<robot_namespace>": 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>": zed_model},
)

# The frame of the point cloud from ignition gazebo 6 isn't provided by <frame_id>.
# 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 <frame_id>.
# 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():
Expand Down

0 comments on commit f49120d

Please sign in to comment.