From f5aaaf52b3d1f75d803971b74c78724a04f93c7b Mon Sep 17 00:00:00 2001 From: Huang77 <2366619700@qq.com> Date: Thu, 13 Aug 2026 17:29:24 +0800 Subject: [PATCH] update --- .../turtlebot3_gazebo/CMakeLists.txt | 8 +- .../config/turtlebot3_gz_bridge.yaml | 9 +- .../launch/depth_cloud.launch.py | 35 ----- .../launch/turtlebot3_office.launch.py | 57 +++----- .../models/turtlebot3_burger/model_gz.sdf | 38 ++++-- .../scripts/depth_to_pointcloud.py | 125 ------------------ 6 files changed, 59 insertions(+), 213 deletions(-) delete mode 100644 src/turtlebot3_simulations/turtlebot3_gazebo/launch/depth_cloud.launch.py delete mode 100644 src/turtlebot3_simulations/turtlebot3_gazebo/scripts/depth_to_pointcloud.py diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt b/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt index 1b46d1d..4b856ef 100755 --- a/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/CMakeLists.txt @@ -118,10 +118,10 @@ install(DIRECTORY launch config models rviz urdf worlds scripts gui DESTINATION share/${PROJECT_NAME}/ ) -# Install Python scripts with correct permissions -install(PROGRAMS scripts/scan_frame_fix.py scripts/camera_topic_remap.py scripts/depth_to_pointcloud.py - DESTINATION lib/${PROJECT_NAME} -) +# Install Python scripts with correct permissions +install(PROGRAMS scripts/scan_frame_fix.py scripts/camera_topic_remap.py + DESTINATION lib/${PROJECT_NAME} +) install(DIRECTORY include/ DESTINATION include/ diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml b/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml index 7727668..8060b00 100644 --- a/src/turtlebot3_simulations/turtlebot3_gazebo/config/turtlebot3_gz_bridge.yaml +++ b/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) === -# Gazebo depth camera with L16 publishes on /depth_image topic -# as gz.msgs.Image type (NOT /image). This bypasses unsupported gz.msgs.DepthImage. +- ros_topic_name: /intel_realsense_r200_pointcloud + ros_type_name: sensor_msgs/msg/PointCloud2 + 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 diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/depth_cloud.launch.py b/src/turtlebot3_simulations/turtlebot3_gazebo/launch/depth_cloud.launch.py deleted file mode 100644 index f4ac8d2..0000000 --- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/depth_cloud.launch.py +++ /dev/null @@ -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 diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office.launch.py b/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office.launch.py index e05476d..bd7dda3 100755 --- a/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office.launch.py +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office.launch.py @@ -19,22 +19,20 @@ 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.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node def generate_launch_description(): launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch') - 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') + 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') pkg_share_dir = get_package_share_directory('turtlebot3_gazebo') world = os.path.join(pkg_share_dir, 'worlds', 'office_gz_dartsim.sdf') @@ -85,33 +83,18 @@ 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', - value=office_models_path + os.pathsep + models_path + ld = LaunchDescription() + + # Set GZ_SIM_RESOURCE_PATH so gz sim can resolve model:// URIs + ld.add_action(SetEnvironmentVariable( + name='GZ_SIM_RESOURCE_PATH', + value=office_models_path + os.pathsep + models_path )) # Add the commands to the launch description - ld.add_action(gz_sim_cmd) - ld.add_action(robot_state_publisher_cmd) - ld.add_action(spawn_turtlebot_cmd) - ld.add_action(scan_frame_fix_cmd) - - return ld + ld.add_action(gz_sim_cmd) + ld.add_action(robot_state_publisher_cmd) + ld.add_action(spawn_turtlebot_cmd) + ld.add_action(scan_frame_fix_cmd) + return ld diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf index 8dd48a1..d79bd64 100755 --- a/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_burger/model_gz.sdf @@ -341,11 +341,11 @@ - - camera_rgb_frame - true - true - 30 + + camera_rgb_frame + true + true + 30 0.076 0.0 0.093 0 0 0 1.02974 @@ -364,10 +364,30 @@ gaussian 0.0 0.007 - - - - + + + + + + + realsense_depth_frame + true + false + 30 + intel_realsense_r200_pointcloud + + 1.02974 + + 640 + 480 + + + 0.02 + 10 + + + + base_footprint diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/scripts/depth_to_pointcloud.py b/src/turtlebot3_simulations/turtlebot3_gazebo/scripts/depth_to_pointcloud.py deleted file mode 100644 index 680682a..0000000 --- a/src/turtlebot3_simulations/turtlebot3_gazebo/scripts/depth_to_pointcloud.py +++ /dev/null @@ -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()