update

src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt

@@ -119,7 +119,7 @@ install(DIRECTORY launch config models rviz urdf worlds scripts gui
 )
 
 # Install Python scripts with correct permissions
-install(PROGRAMS scripts/scan_frame_fix.py scripts/camera_topic_remap.py scripts/depth_to_pointcloud.py
+install(PROGRAMS scripts/scan_frame_fix.py scripts/camera_topic_remap.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml

@@ -35,9 +35,12 @@
   gz_type_name: gz.msgs.CameraInfo
   direction: GZ_TO_ROS
 
-# === Depth image (L16 workaround for Humble) ===
+- ros_topic_name: /intel_realsense_r200_pointcloud
-# Gazebo depth camera with <format>L16</format> publishes on /depth_image topic
+  ros_type_name: sensor_msgs/msg/PointCloud2
-# as gz.msgs.Image type (NOT /image). This bypasses unsupported gz.msgs.DepthImage.
+  gz_topic_name: /world/default/model/burger/link/realsense_link/sensor/intel_realsense_r200_pointcloud/points
+  gz_type_name: gz.msgs.PointCloudPacked
+  direction: GZ_TO_ROS
+
 - ros_topic_name: /intel_realsense_r200_depth/depth/image_raw
   ros_type_name: sensor_msgs/msg/Image
   gz_topic_name: /world/default/model/burger/link/realsense_link/sensor/intel_realsense_r200_depth/depth_image

src/turtlebot3_simulations/turtlebot3_gazebo/launch/depth_cloud.launch.py

@@ -1,35 +0,0 @@
-#!/usr/bin/env python3
-#
-# Launch depth image -> point cloud conversion for TurtleBot3 Gz Sim.
-# Subscribes to the bridged L16 depth image and camera_info, and publishes
-# a sensor_msgs/PointCloud2 that can be visualized in RViz.
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-
-def generate_launch_description():
-    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
-
-    declare_use_sim_time = DeclareLaunchArgument(
-        'use_sim_time',
-        default_value='true',
-        description='Use simulation clock if true')
-
-    # Custom Python replacement for depth_image_proc/point_cloud_xyz.
-    # It converts the bridged L16 depth image + camera_info into a PointCloud2.
-    depth_to_cloud_node = Node(
-        package='turtlebot3_gazebo',
-        executable='depth_to_pointcloud.py',
-        name='depth_to_pointcloud',
-        parameters=[{'use_sim_time': use_sim_time}],
-        output='screen',
-    )
-
-    ld = LaunchDescription()
-    ld.add_action(declare_use_sim_time)
-    ld.add_action(depth_to_cloud_node)
-
-    return ld

src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office.launch.py

@@ -21,10 +21,9 @@ import os
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable
+from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable
-from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
 
 
@@ -34,7 +33,6 @@ def generate_launch_description():
     use_sim_time = LaunchConfiguration('use_sim_time', default='true')
     x_pose = LaunchConfiguration('x_pose', default='0.0')
     y_pose = LaunchConfiguration('y_pose', default='0.0')
-    use_depth_cloud = LaunchConfiguration('use_depth_cloud', default='true')
 
     pkg_share_dir = get_package_share_directory('turtlebot3_gazebo')
     world = os.path.join(pkg_share_dir, 'worlds', 'office_gz_dartsim.sdf')
@@ -85,22 +83,8 @@ def generate_launch_description():
         parameters=[{'use_sim_time': True}],
     )
 
-    depth_cloud_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(launch_file_dir, 'depth_cloud.launch.py')
-        ),
-        launch_arguments={'use_sim_time': use_sim_time}.items(),
-        condition=IfCondition(PythonExpression(["'", use_depth_cloud, "' == 'true'"])),
-    )
-
     ld = LaunchDescription()
 
-    declare_use_depth_cloud_cmd = DeclareLaunchArgument(
-        'use_depth_cloud',
-        default_value='true',
-        description='Launch depth image -> point cloud conversion node'
-    )
-
     # Set GZ_SIM_RESOURCE_PATH so gz sim can resolve model:// URIs
     ld.add_action(SetEnvironmentVariable(
         name='GZ_SIM_RESOURCE_PATH',
@@ -113,5 +97,4 @@ def generate_launch_description():
     ld.add_action(robot_state_publisher_cmd)
     ld.add_action(spawn_turtlebot_cmd)
     ld.add_action(scan_frame_fix_cmd)
-
     return ld

src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf

@@ -367,6 +367,26 @@
           </noise>
         </camera>
       </sensor>
+
+      <!-- Native RGBD sensor for point cloud output. -->
+      <sensor name="intel_realsense_r200_pointcloud" type="rgbd_camera">
+        <gz_frame_id>realsense_depth_frame</gz_frame_id>
+        <always_on>true</always_on>
+        <visualize>false</visualize>
+        <update_rate>30</update_rate>
+        <topic>intel_realsense_r200_pointcloud</topic>
+        <camera name="realsense_pointcloud_camera">
+          <horizontal_fov>1.02974</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>10</far>
+          </clip>
+        </camera>
+      </sensor>
     </link>
 
     <joint name="base_joint" type="fixed">

src/turtlebot3_simulations/turtlebot3_gazebo/scripts/depth_to_pointcloud.py

@@ -1,125 +0,0 @@
-#!/usr/bin/env python3
-#
-# Convert a depth image (16UC1 mm or 32FC1 m) + CameraInfo into a sensor_msgs/PointCloud2.
-# This replaces depth_image_proc/point_cloud_xyz for environments where that package
-# is not available or its executable is missing.
-
-import numpy as np
-import rclpy
-from rclpy.node import Node
-from sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField
-from std_msgs.msg import Header
-
-
-class DepthToPointCloud(Node):
-    def __init__(self):
-        super().__init__('depth_to_pointcloud')
-
-        self.declare_parameter('depth_topic', '/intel_realsense_r200_depth/depth/image_raw')
-        self.declare_parameter('camera_info_topic', '/intel_realsense_r200_depth/camera_info')
-        self.declare_parameter('points_topic', '/intel_realsense_r200_depth/depth/color/points')
-        self.declare_parameter('max_depth_m', 10.0)
-
-        depth_topic = self.get_parameter('depth_topic').value
-        camera_info_topic = self.get_parameter('camera_info_topic').value
-        points_topic = self.get_parameter('points_topic').value
-        self.max_depth_m = float(self.get_parameter('max_depth_m').value)
-
-        self.camera_info = None
-
-        self.info_sub = self.create_subscription(
-            CameraInfo, camera_info_topic, self.info_callback, qos_profile=1)
-        self.depth_sub = self.create_subscription(
-            Image, depth_topic, self.depth_callback, qos_profile=1)
-        self.cloud_pub = self.create_publisher(PointCloud2, points_topic, qos_profile=1)
-
-        self.get_logger().info(
-            f'Waiting for camera info on {camera_info_topic}; '
-            f'publishing points on {points_topic}'
-        )
-
-    def info_callback(self, msg: CameraInfo):
-        self.camera_info = msg
-
-    def depth_callback(self, msg: Image):
-        if self.camera_info is None:
-            self.get_logger().warn(
-                'CameraInfo not received yet; skipping depth frame',
-                throttle_duration_sec=5)
-            return
-
-        # Decode depth image according to its encoding.
-        # Gz Sim may publish depth as either 16-bit unsigned mm (16UC1/mono16)
-        # or 32-bit float meters (32FC1), depending on sensor/bridge version.
-        if msg.encoding in ('16UC1', 'mono16'):
-            depth_raw = np.frombuffer(msg.data, dtype=np.uint16).reshape(msg.height, msg.width)
-            depth_m = depth_raw.astype(np.float32) * 0.001
-        elif msg.encoding == '32FC1':
-            depth_m = np.frombuffer(msg.data, dtype=np.float32).reshape(msg.height, msg.width)
-        else:
-            self.get_logger().error(
-                f'Unsupported depth encoding: {msg.encoding}; expected 16UC1/mono16 or 32FC1',
-                throttle_duration_sec=5)
-            return
-
-        # Intrinsics from CameraInfo
-        K = self.camera_info.k
-        fx, fy = float(K[0]), float(K[4])
-        cx, cy = float(K[2]), float(K[5])
-
-        # Pixel coordinates
-        u = np.arange(msg.width, dtype=np.float32)
-        v = np.arange(msg.height, dtype=np.float32)
-        u, v = np.meshgrid(u, v)
-
-        # Filter valid depth pixels
-        valid = (depth_m > 0.0) & (depth_m < self.max_depth_m)
-        u = u[valid]
-        v = v[valid]
-        z = depth_m[valid]
-
-        if z.size == 0:
-            return
-
-        # Back-project to 3D (in the camera optical frame)
-        x = (u - cx) * z / fx
-        y = (v - cy) * z / fy
-
-        points = np.stack((x, y, z), axis=-1).astype(np.float32)
-
-        header = Header()
-        header.stamp = msg.header.stamp
-        header.frame_id = self.camera_info.header.frame_id
-
-        cloud_msg = PointCloud2()
-        cloud_msg.header = header
-        cloud_msg.height = 1
-        cloud_msg.width = int(points.shape[0])
-        cloud_msg.fields = [
-            PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
-            PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
-            PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
-        ]
-        cloud_msg.is_bigendian = False
-        cloud_msg.point_step = 12
-        cloud_msg.row_step = cloud_msg.point_step * cloud_msg.width
-        cloud_msg.is_dense = True
-        cloud_msg.data = points.tobytes()
-
-        self.cloud_pub.publish(cloud_msg)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = DepthToPointCloud()
-    try:
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == '__main__':
-    main()