深度相机(RGB-D Camera)同时输出彩色图像和每像素的深度信息。常见方案包括结构光(RealSense)、飞行时间(ToF)和双目立体视觉。
#!/usr/bin/env python3
# pointcloud_processor.py - 点云处理节点
# ✅ Docker验证通过
import rclpy, struct, numpy as np
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
from geometry_msgs.msg import Twist
from std_msgs.msg import Header
class PointCloudProcessor(Node):
def __init__(self):
super().__init__('pointcloud_processor')
self.declare_parameter('min_height', 0.1) # 最小高度(过滤地面)
self.declare_parameter('max_height', 2.0) # 最大高度
self.declare_parameter('min_distance', 0.3) # 最小距离
self.declare_parameter('max_distance', 5.0) # 最大距离
self.declare_parameter('voxel_size', 0.05) # 体素滤波大小
self.declare_parameter('safe_distance', 0.8) # 安全距离
self.sub = self.create_subscription(
PointCloud2, '/camera/depth/points', self._cb, 10)
self.filtered_pub = self.create_publisher(PointCloud2, '/points_filtered', 10)
self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.get_logger().info('🔲 点云处理器已启动')
def _cb(self, msg: PointCloud2):
# 解析PointCloud2
points = self._parse_cloud(msg)
if points is None: return
# 过滤：高度范围 + 距离范围
min_h = self.get_parameter('min_height').value
max_h = self.get_parameter('max_height').value
min_d = self.get_parameter('min_distance').value
max_d = self.get_parameter('max_distance').value
# 地面分割：保留高于地面的点
mask = (points[:,2] > min_h) & (points[:,2] < max_h)
filtered = points[mask]
# 距离过滤
dist = np.sqrt(filtered[:,0]**2 + filtered[:,1]**2)
mask2 = (dist > min_d) & (dist < max_d)
filtered = filtered[mask2]
if len(filtered) == 0: return
# 前方障碍物检测
front = filtered[(np.abs(filtered[:,1]) < 0.5)] # ±0.5m范围
if len(front) > 0:
min_front_dist = np.min(np.sqrt(front[:,0]**2 + front[:,1]**2))
safe_d = self.get_parameter('safe_distance').value
if min_front_dist < safe_d:
self.get_logger().warn(f'⚠️ 前方3D障碍: {min_front_dist:.2f}m')
# 发布过滤后的点云
filtered_msg = self._create_cloud(msg.header, filtered)
self.filtered_pub.publish(filtered_msg)
def _parse_cloud(self, msg):
# 解析xyz字段
fmt = 'fff' # x, y, z
point_step = msg.point_step
points = []
for i in range(0, len(msg.data), point_step):
x, y, z = struct.unpack_from(fmt, msg.data, i)
if np.isfinite(x) and np.isfinite(y) and np.isfinite(z):
points.append([x, y, z])
return np.array(points) if points else None
def _create_cloud(self, header, points):
msg = PointCloud2()
msg.header = header
msg.height = 1
msg.width = len(points)
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),
]
msg.is_bigendian = False
msg.point_step = 12
msg.row_step = 12 * len(points)
msg.data = points.astype(np.float32).tobytes()
msg.is_dense = True
return msg
def main(args=None):
rclpy.init(args=args); rclpy.spin(PointCloudProcessor()); rclpy.shutdown()#!/usr/bin/env python3
# depth_to_costmap.py - 深度图像转局部代价地图
import rclpy, numpy as np
from rclpy.node import Node
from sensor_msgs.msg import Image
from nav_msgs.msg import OccupancyGrid
from cv_bridge import CvBridge
import math
class DepthToCostmap(Node):
def __init__(self):
super().__init__('depth_to_costmap')
self.declare_parameter('map_resolution', 0.05) # m/cell
self.declare_parameter('map_width', 3.0) # m
self.declare_parameter('map_height', 3.0) # m
self.declare_parameter('obstacle_height', 0.15) # m
self.declare_parameter('camera_height', 0.5) # m
self.declare_parameter('fov_h', 1.047) # 60° horizontal
self.bridge = CvBridge()
self.res = self.get_parameter('map_resolution').value
self.w = int(self.get_parameter('map_width').value / self.res)
self.h = int(self.get_parameter('map_height').value / self.res)
self.obs_h = self.get_parameter('obstacle_height').value
self.cam_h = self.get_parameter('camera_height').value
self.depth_sub = self.create_subscription(
Image, '/camera/depth/image_raw', self._cb, 10)
self.costmap_pub = self.create_publisher(OccupancyGrid, '/depth_costmap', 10)
self.get_logger().info('🔲 深度代价地图生成器已启动')
def _cb(self, msg: Image):
depth = self.bridge.imgmsg_to_cv2(msg, desired_encoding='16UC1')
# 生成代价地图
grid = OccupancyGrid()
grid.header = msg.header
grid.info.resolution = self.res
grid.info.width = self.w
grid.info.height = self.h
grid.info.origin.position.x = -self.get_parameter('map_width').value / 2
grid.info.origin.position.y = -self.get_parameter('map_height').value / 2
data = [-1] * (self.w * self.h) # 初始未知
for v in range(0, depth.shape[0], 4): # 4倍降采样
for u in range(0, depth.shape[1], 4):
d = depth[v, u] / 1000.0 # mm→m
if d < 0.1 or d > 5.0: continue
# 像素→3D坐标
fov = self.get_parameter('fov_h').value
angle_x = (u / depth.shape[1] - 0.5) * fov
angle_y = (v / depth.shape[0] - 0.5) * fov
x3d = d * math.cos(angle_y) * math.sin(angle_x)
y3d = d * math.sin(angle_y)
z3d = d * math.cos(angle_y) * math.cos(angle_x)
# 地面过滤
world_z = self.cam_h + y3d
if world_z < self.obs_h: continue # 地面点，跳过
# 3D→栅格坐标
gx = int((x3d - grid.info.origin.position.x) / self.res)
gy = int((z3d - grid.info.origin.position.y) / self.res)
if 0 <= gx < self.w and 0 <= gy < self.h:
data[gy * self.w + gx] = 100 # 障碍物
grid.data = data
self.costmap_pub.publish(grid)
def main(args=None):
rclpy.init(args=args); rclpy.spin(DepthToCostmap()); rclpy.shutdown()// pointcloud_core.cpp
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include <vector><cmath>
class PointCloudCore : public rclcpp::Node {
public:
PointCloudCore() : Node("pointcloud_core") {
sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
"/camera/depth/points", 10,
[this](sensor_msgs::msg::PointCloud2::SharedPtr msg) {
analyze(msg);
});
RCLCPP_INFO(get_logger(), "C++点云处理器已启动");
}
private:
void analyze(const sensor_msgs::msg::PointCloud2::SharedPtr& msg) {
// 找到x,y,z字段的偏移
size_t x_off=0, y_off=0, z_off=0;
for (auto& f : msg->fields) {
if (f.name == "x") x_off = f.offset;
if (f.name == "y") y_off = f.offset;
if (f.name == "z") z_off = f.offset;
}
int obs_count = 0;
double min_front = 999.0;
for (size_t i = 0; i < msg->width; i++) {
const float* xp = reinterpret_cast<const float*>(
msg->data.data() + i * msg->point_step + x_off);
const float* yp = reinterpret_cast<const float*>(
msg->data.data() + i * msg->point_step + y_off);
const float* zp = reinterpret_cast<const float*>(
msg->data.data() + i * msg->point_step + z_off);
if (std::isfinite(*xp) && *yp > 0.1) { // 高于地面
double dist = std::sqrt((*xp)*(*xp) + (*zp)*(*zp));
if (dist < min_front && std::abs(*yp) < 0.5)
min_front = dist;
obs_count++;
}
}
if (min_front < 0.8)
RCLCPP_WARN(get_logger(), "前方3D障碍: %.2fm", min_front);
}
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
};调整min_height参数，观察地面分割效果。太高会怎样？太低呢？
运行DepthToCostmap，在rviz2中查看生成的代价地图。
实现体素下采样滤波，对比原始和滤波后的点云数量。
经验值：+250 XP