diff --git a/gzsim_run.sh b/gzsim_run.sh index 8ed346d..09b10df 100755 --- a/gzsim_run.sh +++ b/gzsim_run.sh @@ -16,6 +16,16 @@ source /opt/ros/humble/setup.bash source "${ROOT}/install/setup.bash" set -u +# The container injects /opt/vendor/python (numpy built for Python 3.12) into +# PYTHONPATH, but ROS 2 Humble uses system Python 3.10. This mismatch crashes +# every ROS 2 Python node that imports numpy (sensor_msgs, our depth_to_pointcloud +# node, scan_frame_fix, etc.). Strip vendor Python paths before launching ROS 2. +if [[ -n "${PYTHONPATH:-}" ]]; then + PYTHONPATH=$(echo "$PYTHONPATH" | tr ':' '\n' | grep -v '^/opt/vendor/python' | paste -sd ':' -) + export PYTHONPATH + echo "[autorun] sanitized PYTHONPATH (removed /opt/vendor/python entries)" +fi + LOG_DIR="${ROOT}/autorun_logs/autorun_$(date +%Y%m%d_%H%M%S)" mkdir -p "$LOG_DIR" diff --git a/src/turtlebot3_simulations/turtlebot3_gazebo/scripts/depth_to_pointcloud.py b/src/turtlebot3_simulations/turtlebot3_gazebo/scripts/depth_to_pointcloud.py index d086c2e..680682a 100644 --- a/src/turtlebot3_simulations/turtlebot3_gazebo/scripts/depth_to_pointcloud.py +++ b/src/turtlebot3_simulations/turtlebot3_gazebo/scripts/depth_to_pointcloud.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 # -# Convert a 16-bit (L16 / 16UC1) depth image + CameraInfo into a sensor_msgs/PointCloud2. +# 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. @@ -48,15 +48,20 @@ class DepthToPointCloud(Node): throttle_duration_sec=5) return - if msg.encoding not in ('16UC1', 'mono16'): + # 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') + f'Unsupported depth encoding: {msg.encoding}; expected 16UC1/mono16 or 32FC1', + throttle_duration_sec=5) return - # Decode L16 depth image (mm -> m) - depth_mm = np.frombuffer(msg.data, dtype=np.uint16).reshape(msg.height, msg.width) - depth_m = depth_mm.astype(np.float32) * 0.001 - # Intrinsics from CameraInfo K = self.camera_info.k fx, fy = float(K[0]), float(K[4])