工作记录-ROS雷达开发示例(改变颜色)

1 天 ago

木它

2 minutes

前提

雷达驱动已经在发布点云数据（ /cx/lslidar_point_cloud）

Python实现步骤（简单示例）

a. 安装依赖

确保你有pcl_ros、sensor_msgs、rospy。

新建包

cd ~/catkin_ws/src
catkin_create_pkg colorize_cloud rospy std_msgs sensor_msgs pcl_ros

然后新建文件夹

cd ~/catkin_ws/src/colorize_cloud
mkdir scripts launch

a. 目录结构

colorize_cloud/
├── CMakeLists.txt
├── package.xml
├── launch/
│   └── colorize_cloud.launch
└── scripts/
    └── colorize_cloud.py

b. 新建节点代码

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2

def colorize_point_cloud(msg):
    # 收到原始点云
    new_points = []
    for p in pc2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True):
        x, y, z = p[0], p[1], p[2]
        distance = (x ** 2 + y ** 2 + z ** 2) ** 0.5
        # 设定区间，比如距离在3~5米的点变红
        if 3 < distance < 5:
            # RGB转float
            r, g, b = 255, 0, 0
            rgb = (r << 16) | (g << 8) | b
            rgb = struct.unpack('f', struct.pack('I', rgb))[0]
        else:
            r, g, b = 0, 255, 0
            rgb = (r << 16) | (g << 8) | b
            rgb = struct.unpack('f', struct.pack('I', rgb))[0]
        new_points.append([x, y, z, rgb])

    fields = [
        PointField('x', 0, PointField.FLOAT32, 1),
        PointField('y', 4, PointField.FLOAT32, 1),
        PointField('z', 8, PointField.FLOAT32, 1),
        PointField('rgb', 12, PointField.FLOAT32, 1)
    ]
    header = msg.header
    pc2_msg = pc2.create_cloud(header, fields, new_points)
    pub.publish(pc2_msg)

if __name__ == '__main__':
    import struct
    rospy.init_node('colorize_cloud')
    pub = rospy.Publisher('/colorized_cloud', PointCloud2, queue_size=1)
    rospy.Subscriber('/cx/lslidar_point_cloud', PointCloud2, colorize_point_cloud)
    rospy.spin()

c. 使用RViz显示颜色

在RViz中(添加)订阅 /colorized_cloud 话题。
在点云显示属性里，将颜色模式设置为“RGB”。

4. 启动方法

保存脚本为 scripts/colorize_cloud.py，加执行权限
chmod +x colorize_cloud.py
在launch文件中添加节点：

<node pkg="colorize_cloud" type="colorize_cloud.py" name="colorize_cloud" output="screen"/>

每次新建包或改了 CMakeLists.txt 都要编译一次：
启动你的launch文件

cd ~/catkin_ws
catkin_make
source devel/setup.bash

# 启动雷达驱动
roslaunch lslidar_driver lslidar_cx.launch

# 启动新包
rosrun colorize_cloud colorize_cloud.py

3. 启动你的着色节点

方法一：直接用rosrun

rosrun colorize_cloud colorize_cloud.py

方法二：用launch文件启动
比如你建了 colorize_cloud/launch/colorize_cloud.launch，内容如下：

<launch>
  <node pkg="colorize_cloud" type="colorize_cloud.py" name="colorize_cloud" output="screen"/>
</launch>

则可以这样启动：

roslaunch colorize_cloud colorize_cloud.launch

5. 效果验证

RViz显示 /colorized_cloud，你会看到距离在3~5米的点变红色，其余为绿色。