发布标注好的点云
发布保存的点云
将可视化后的点云保存为图像
读取点云文件-发布
std_msgs/Header header：数据头，包含该帧点云的时间戳、坐标系等属性信息
uint32 height：data的高度，一帧点云通常height=1，即表示无序点云；
uint32 width：data的宽度，即每行对应点的数量；
sensor_msgs/PointField[] fields：包含每个点的字段属性信息，详见下文。
bool is_bigendian：点云是否按正序排列
uint32 point_step：每个点占用的比特数，1字节=8比特，与PointField里所有字节数之和相等。
uint32 row_step：每行占用的比特数，=点的数量*Point_step;
uint8[] data：序列化后的点云二进制数据，所有点信息都在一串字符中，无法通过data[i]取值。
bool is_dense：是否存在无效点。
示例代码
#! /usr/bin/env python3
# -*- coding=utf-8 -*-
import rospy
from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2
import open3d as o3d
import numpy as np
import os
def talker():
pub = rospy.Publisher('points_b', PointCloud2, queue_size=10)
rospy.init_node('node', anonymous=True)
rate = rospy.Rate(2) # 2hz
##处理PCD点云
pcd_path = '~/pcd'
#得到文件夹下的所有文件名称
files= os.listdir(pcd_path)
#files.sort(key = lambda x: int(x[:-4]))
files.sort()
for i,pcd_file in enumerate(files):
print(i,pcd_file)
pcd = o3d.t.io.read_point_cloud(os.path.join(pcd_path,pcd_file))
exp_points = pcd.point
point_xyz = exp_points['positions'].numpy()
# 构造消息
msg = PointCloud2()
msg.header.stamp = rospy.Time().now()
msg.header.frame_id = "map"
msg.fields = [
PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)]
msg.is_bigendian = False
msg.point_step = 12
msg.row_step = msg.point_step * point_xyz.shape[0]
msg.is_dense = False
if len(point_xyz.shape) == 3:
msg.height = point_xyz.shape[1]
msg.width = point_xyz.shape[0]
else:
msg.height = 1
msg.width = len(point_xyz)
msg.data = np.asarray(point_xyz, np.float32).tostring()
pub.publish(msg)
rate.sleep()
if __name__ == '__main__':
for i in range(20):
talker()
点云保存为图像
vis = o3d.visualization.Visualizer()
vis.create_window()
vis.add_geometry(pcd)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
### 将点云保存为图像
vis.capture_screen_image('test.png')
vis.run()
vis.destroy_window()
###Have you tried: vis.capture_screen_image('test.png', true)?
###Is the window visible and showing the image prior to this function call,
###i.e., try to add time.sleep(5) before the capture_screen_image?
说明
bag函数
bag.open()函数有两个参数：第一个是打开的文件名，如果该文件不存在则创建新文件，；第二个参数是打开方式，打开方式在BagMode里。
bag.write()有三个参数：第一个是发布的话题；第二个参数是时间戳，
一般是选择ros::Time::now()，有的时候也可以用ros::Time(0.0)，
最后一个参数是消息，注意要包std_msgs/String.h头文件。
bag.close()关闭文件， 记得一定要关闭，否则会造成内存泄露。
sensor_msgs.msg.PointCloud2
sensor_msgs.msg.PointCloud2 函数 read_points create_cloud
point_cloud2.create_cloud 函数说明
from std_msgs.msg import Header
from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2
pcd = o3d.t.io.read_point_cloud(data_sig)
pcd_intensity = pcd.point["intensity"].numpy() #强度-转换为数组类型
pcd_points = pcd.point["positions"].numpy() #坐标 -转换为数组类型
complex_points = np.zeros(( pcd_points.shape[0],4))
complex_points[:,:3] = pcd_points
complex_points[:,3] = pcd_intensity.reshape(1,-1)
pc2_msg = point_cloud2.create_cloud(header, msg_fields, complex_points)
for topic, msg, t in tqdm(bag.read_messages(topics=topic)):
cloud = np.asarray(list(point_cloud2.read_points(msg, field_names=['x', 'y', 'z', "intensity"])))
pcd_obj = o3d.t.geometry.PointCloud()
pcd_obj.point['positions'] = o3d.core.Tensor(cloud[:, :3],dtype=o3d.core.Dtype.Float32)
pcd_obj.point['intensity'] = o3d.core.Tensor(cloud[:, 3].reshape(-1,1),dtype=o3d.core.Dtype.Float32)
排序
sorted() 函数对序列进行排序， 并不会在原序列的基础进行修改，而是会重新生成一个排好序的列表
exp_list = sorted(iterable, key=None, reverse=False)
#自定义按照字符串长度排序 sorted(chars,key=lambda x:len(x))
ex_list.sort(key=lambda x: float(x.split("_")[-1][0:-5]))
示例代码
#!/usr/bin/python3
# -*- coding: utf-8 -*-
import os
import roslib
import rospy
roslib.load_manifest('sensor_msgs')
import numpy as np
import open3d as o3d
from ros import rosbag
from std_msgs.msg import Header
from sensor_msgs.msg import PointField
from sensor_msgs.msg import PointCloud2
from sensor_msgs.point_cloud2 import create_cloud
'''get pcd from dir'''
def get_pcd(pcd_dir):
'''Generates a list of files from the directory'''
print( "Searching directory %s" % pcd_dir )
pcd_files = []
pcd_timestamps =[]
if os.path.exists(pcd_dir):
for path, names, files in os.walk(pcd_dir ):
for f in sorted(files):
if os.path.splitext(f)[1] in ['.pcd']:
file_nm =os.path.join( path, f )
pcd_files.append( file_nm )
time_img =os.path.splitext(f)[0]
pcd_timestamps.append(time_img)
return pcd_files,pcd_timestamps
def creat_pcd_rosbag(pcd_dir,out_bag_nm):
pts_dtype = PointField.FLOAT32
msg_fields = [PointField(name='x', offset=0, datatype=pts_dtype, count=1),
PointField(name='y', offset=4, datatype=pts_dtype, count=1),
PointField(name='z', offset=8, datatype=pts_dtype, count=1),
PointField(name='intensity', offset=12, datatype=pts_dtype, count=1) ]
bag = rosbag.Bag(out_bag_nm, 'w')
pcd_data = get_pcd(pcd_dir)
all_pcd_file = pcd_data[0]
all_time_file = pcd_data[1]
for i,data_sig in enumerate(all_pcd_file) :
print("Adding {} {} ".format(i,data_sig ))
pcd = o3d.t.io.read_point_cloud(data_sig)
pcd_intensity = pcd.point["intensity"].numpy() #强度-转换为数组类型
pcd_points = pcd.point["positions"].numpy() #坐标 -转换为数组类型
complex_points = np.zeros(( pcd_points.shape[0],4))
complex_points[:,:3] = pcd_points
complex_points[:,3] = pcd_intensity.reshape(1,-1)
stamp_time = rospy.rostime.Time.from_sec(float(all_time_file[i]))
header = Header()
header.stamp = stamp_time
header.frame_id = "map"
pc2_msg = create_cloud(header, msg_fields, complex_points)
topic_nm ="/test/cloud_points"
bag.write(topic_nm, pc2_msg, stamp_time)
bag.close()
if __name__ == '__main__':
dir_exp = r"/test/pts/"
bag_nm_exp = r"/test/pts.bag"
creat_pcd_rosbag(dir_exp,bag_nm_exp)
参考
【ros】numpy 格式点云转 sensor_msgs/PointCloud2 格式发布 https://blog.csdn.net/qq_35632833/article/details/108050233
动手学ROS（13）：点云(PointCloud2)的发送与接收--python和c++示例 https://zhuanlan.zhihu.com/p/421691350
ROS(机器人操作系统) http://www.autolabor.com.cn/book/ROSTutorials/
【Open3D】点云可视化 https://zhuanlan.zhihu.com/p/483414760
https://github.com/isl-org/Open3D/issues/1110
Ros_JPG_rosbag提取点云数据_pcd和图像数据_jpg https://www.cnblogs.com/ytwang/p/16087709.html
