使用Python和C将带有颜色的3D点云数据发布到ROS1并在RViz中显示

ros中发布点云数据xyz以及带颜色的点云数据xyzrgb

ros中发布点云数据xyz

可以直接用python来做或者C++(看个人偏好)

在这里我们带有颜色的点云数据格式为x y z c
其中c值为float型，有四种值1.0,2.0,3.0,4.0

代码文件b.py，具体内容如下：

import rospy
from sensor_msgs.msg import Image,PointCloud2
from cv_bridge import CvBridge
import numpy as np
import os
import cv2
from a import *
import open3d as o3d

DATA_PATH='/home/cxh/project/0618/point_cloud_selected.txt'
if __name__=='__main__':
    rospy.init_node('jizhui_node',anonymous=True)
    # cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)
    pcl_pub=rospy.Publisher('/jizhui_pcl',PointCloud2,queue_size=1)#创建点云发布者，queue_size=10表示消息队列的大小
    bridge=CvBridge()#创建一个CvBridge实例，用于在OpenCV图像与ROS图像消息之间进行转换
    # rate=rospy.Rate(1)#设置发布频率为10Hz
    rate=rospy.Rate(5,reset=True)
    frame=0#初始化帧计数器
    def load_point_cloud(file_path):
        """
        从文本文件中加载点云数据
        """
        point_cloud = []
        color=[]
        with open(file_path, 'r') as f:
            for line in f:
                if line.strip():  # 忽略空行
                    try:
                        # 假设每行的格式为: x y z
                        x, y, z, c= map(lambda x: round(float(x) / 100, 5) if x != line.strip().split()[-1] else float(x), line.strip().split())
                        point_cloud.append([x, y, z])
                        #读取c的值，并把c的值映射成对应RGB值
                        # 1.0：灰色[220,220,220]
                        # 2.0：蓝色[173,216,230]
                        # 3.0：黄色[255,215,0]
                        # 4.0：红色[255,182,193]

                        # r, g, b = color_mapping(c)
                        # color.append([r, g, b])
                        # print("x y z",point_cloud)
                    except ValueError:
                        rospy.logerr("Error parsing line: {}".format(line))
        return np.array(point_cloud)
    
    # 循环播放，通过frame控制帧数
    while not rospy.is_shutdown():#主循环在ROS节点关闭前一直运行
        # 读取点云数据    
        point_cloud= load_point_cloud(DATA_PATH)#,color
        # print("shape:",point_cloud.shape)#49行3列

        publish_pcl(point_cloud,pcl_pub,'map')#调用publish_pcl函数将点云数据发布到ROS主题jizhui_pcl。color,
        # pcl_pub.publish(pcl2_msg)
        rospy.loginfo('published')#在日志中记录发布信息
        rate.sleep()#按照设定的频率进行休眠

文件a.py具体内容如下：

from sensor_msgs.msg import PointCloud2,PointField
import sensor_msgs.point_cloud2 as pcl2


from std_msgs.msg import Header
import rospy
import numpy as np
def publish_pcl(point_cloud,pcl_pub,frame_id):#定义点云数据的ROS发布者。
    if point_cloud.size == 0:
        rospy.logwarn("Empty point cloud data, skipping publish.")
        return
    
    header=Header()
    header.stamp=rospy.Time.now()
    header.frame_id=frame_id

    point_cloud_message=pcl2.create_cloud_xyz32(header,point_cloud)

    pcl_pub.publish(point_cloud_message)

发布到ros的步骤如下

#启动 ROS master
#启动一个终端键入
roscore
#在python文件b.py下运行代码
python b.py
#再另起一个终端键入
rviz

启动了 RViz 后点击界面左下角的Add按钮并添加一个 PointCloud2 显示
即可在 RViz 中看到点云了
**注意：**对于发布带颜色的点云数据，由于python版没有creat_xyzrgb32 ，这个功能函数只有C++有，python的需要自己写一个这样的功能函数。我本人也参考b站博主学习视频链接: 用python将着色点云在ros中发布—解析PointCloud2与Rviz可视化源码弄了一下午没成功，就改用C++来做了！！
（ps这里还有一个特别需要注意的点，就是有的点云图很大且高，小窗口下不容易发现，我们一开始就因为这个问题郁闷了很久，一遍遍检查代码，后来发现早就生成了，只是没有吧窗口放大，多拖拉几下就能找到躲在角落处的点云图！！！）

ros中发布带颜色的点云数据xyzrgb

环境

环境:
Ubuntu20.04
ROS noetic
C++

1.新建ROS工作空间

mkdir -p PointCloundShow_ws/src
cd PointCloundShow_ws/src
catkin_init_workspace
cd ..
catkin_make 

2.创建功能包

cd src
catkin_create_pkg pointcloundshow pcl_ros roscpp rospy sensor_msgs std_msgs 

在功能包的src文件夹下新建cpp文件pcl_colored.cpp
代码文件pcl_colored.cpp内容如下：

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <fstream>
#include <sstream>
#include <vector>
#include <iostream>
#include <string>

using namespace std;
uint32_t color_mapping(float c) {
    if (c == 1.0) {
        return (220 << 16) | (220 << 8) | 220;  // 灰色
    } else if (c == 2.0) {
        return (173 << 16) | (216 << 8) | 230;  // 
    } else if (c == 3.0) {
        return (255 << 16) | (215 << 8) | 0;  // 
    } else {
        return (255 << 16) | (182 << 8) | 193;  // 
    }
}

void readPointCloudFromFile(const string& filename, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud) {
    ifstream infile(filename);
    if (!infile.is_open()) {
        cerr << "Could not open file: " << filename << endl;
        return;
    }

    string line;
    while (getline(infile, line)) {
        istringstream iss(line);
        float x, y, z, c;
        if (!(iss >> x >> y >> z >> c)) {
            cerr << "Error reading line: " << line << endl;
            continue;
        }
        // 缩小 x, y, z 的值100倍
        x /= 100.0f;
        y /= 100.0f;
        z /= 100.0f;

        pcl::PointXYZRGB point;
        point.x = x;
        point.y = y;
        point.z = z;

        uint32_t rgb = color_mapping(c);
        point.rgb = *reinterpret_cast<float*>(&rgb);
        
        cloud->points.push_back(point);
    }
    cloud->width = cloud->points.size();
    cloud->height = 1;
    cloud->is_dense = true;

    infile.close();
}
int main(int argc, char** argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "pcl_create_xyzrgb");
    ros::NodeHandle nh;

    // 创建一个发布者
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("pcl_output_colored", 1);

    // 创建一个点云对象
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());

    // 从文件读取点云数据
    string filename = "/home/cxh/project/0618/point_cloud_selected.txt";
    readPointCloudFromFile(filename, cloud);

    // 将点云数据转换为ROS消息
    sensor_msgs::PointCloud2 output;
    pcl::toROSMsg(*cloud, output);
    output.header.frame_id = "map";

    // 发布点云消息
    ros::Rate loop_rate(1);
    while (ros::ok()) {
        output.header.stamp = ros::Time::now();
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

然后将下面的编译规则写到功能包的CMakeLists.txt文件中

find_package(PCL REQUIRED) 
include_directories(include${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS}) 
add_executable(pcl_colored.cpp src/pcl_colored.cpp)
target_link_libraries(pcl_colored.cpp ${catkin_LIBRARIES} ${PCL_LIBRARIES})

翻倒CMakeLists.txt文件的最后一行添加，我们的如下:

如图所示，因为我们用C++写了两个文件，一个是pcl_create.cpp另一个是目前的pcl_colored.cpp，所以我们这个是追加到后面的，供参考！

回到工作空间路径下也就是PointCloundShow_ws，输入catkin_make进行编译
然后再更新一下:source devel/setup.bash（**注意：**只要你修改过你工作空间任何一处代码，每次都需要重新编译和更新！！！另外每新建了一个cpp文件都需要在CMakeLists.txt文件做添加！！！）
然后新起一个终端终端运行roscore指令，
再在刚的source命令那个终端运行rosrun pointcloundshow pcl_create
最后再另起一个终端允许rviz指令

打开rviz
在rviz中左下角点Add增加PointCloud2d
topic 选 /pcl_output
fixed Frame 输入odom
或者直接点左下角的Add然后会弹出一个名字为rviz的框，选择By topic下的对应发布的点云名字
即可看到发布的带有颜色的点云图

作者：double_c之