从.pcd读取Pointcloud到ROS PointCloud2

Question

我想创建一个简单的python脚本来读取一些.pcd文件，并在rosbag中为每个文件创建一个sensor_msgs::PointCloud2。

我尝试使用python-pcl库，但在向数据字段添加点时，我可能会做错一些事情，因为在播放rosbag并使用RViz检查并响应主题时，我没有得到点。

这是我设置PointCloud2 msg的部分。

pcl_data = pcl.load(metadata_dir + "/" + pcd_path)
# get data
pcl_msg = sensor_msgs.msg.PointCloud2()
pcl_msg.data = np.ndarray.tobytes(pcl_data.to_array())
pcl_msg.header.stamp = rospy.Time(t_us/10000000.0)
pcl_msg.header.frame_id = "robot_1/navcam_sensor"
# Pusblish Pointcloud2 msg
outbag.write("/robot_1/pcl_navcam", pcl_msg, rospy.Time(t_us/10000000.0))

我也尝试过pypc，但没有任何运气。

你会怎么做?也许在pcl的cpp版本中有一个ToROSMsg方法?

对于cpp: pcl::toROSMsg中非常容易获得的内容，是否有对应的python ?

谢谢以下是python脚本的完整代码:

#! /usr/bin/env python3
import rospy
import rosbag
import tf2_msgs.msg
import geometry_msgs.msg
import sensor_msgs.msg
import sys
import os
import json
import numpy as np
import tf.transformations as tf_transformations
import pcl
import json
import math
import pypcd
import sensor_msgs.point_cloud2 as pc2
import tf2_msgs.msg._TFMessage

def main():
output_bag_path = dataset_path + "rosbag.bag"
with rosbag.Bag(output_bag_path, 'w') as outbag:

# iterate metadata files with tfs
metadata_dir = dataset_path + "Pointcloud/metadata"
t_first_flag = False
# for filename in os.listdir(metadata_dir):
list_of_files = sorted( filter( lambda x: os.path.isfile(os.path.join(metadata_dir, x)),
os.listdir(metadata_dir) ) )
for filename in list_of_files:
# open json file
json_path = os.path.join(metadata_dir, filename)
json_file = open(json_path)
json_data = json.load(json_file)
# get timestamp 
t_us = json_data 
["metadata"] 
["Timestamps"] 
["microsec"]
t_ns, t_s = math.modf(t_us/1000000)
# get camera tf
pos = geometry_msgs.msg.Vector3( 
json_data["metadata"] 
["pose_robotFrame_sensorFrame"] 
["data"] 
["translation"][0], 
json_data["metadata"] 
["pose_robotFrame_sensorFrame"] 
["data"] 
["translation"][1], 
json_data["metadata"] 
["pose_robotFrame_sensorFrame"] 
["data"] 
["translation"][2])
quat = geometry_msgs.msg.Quaternion( 
json_data["metadata"] 
["pose_robotFrame_sensorFrame"] 
["data"] 
["orientation"] 
["x"], 
json_data["metadata"] 
["pose_robotFrame_sensorFrame"] 
["data"] 
["orientation"] 
["y"], 
json_data["metadata"] 
["pose_robotFrame_sensorFrame"] 
["data"] 
["orientation"] 
["z"], 
json_data["metadata"] 
["pose_robotFrame_sensorFrame"] 
["data"] 
["orientation"] 
["w"], )
navcam_sensor_tf = geometry_msgs.msg.TransformStamped()
navcam_sensor_tf.header.frame_id = "reu_1/base_link"
navcam_sensor_tf.child_frame_id = "reu_1/navcam_sensor"
navcam_sensor_tf.header.stamp = rospy.Time(t_us/1000000.0)
navcam_sensor_tf.transform.translation = pos
navcam_sensor_tf.transform.rotation = quat
# get base_link tf
pos = geometry_msgs.msg.Vector3( 
json_data["metadata"] 
["pose_fixedFrame_robotFrame"] 
["data"] 
["translation"][0], 
json_data["metadata"] 
["pose_fixedFrame_robotFrame"] 
["data"] 
["translation"][1], 
json_data["metadata"] 
["pose_fixedFrame_robotFrame"] 
["data"] 
["translation"][2])
quat = geometry_msgs.msg.Quaternion( 
json_data["metadata"] 
["pose_fixedFrame_robotFrame"] 
["data"] 
["orientation"] 
["x"], 
json_data["metadata"] 
["pose_fixedFrame_robotFrame"] 
["data"] 
["orientation"] 
["y"], 
json_data["metadata"] 
["pose_fixedFrame_robotFrame"] 
["data"] 
["orientation"] 
["z"], 
json_data["metadata"] 
["pose_fixedFrame_robotFrame"] 
["data"] 
["orientation"] 
["w"], )
base_link_tf = geometry_msgs.msg.TransformStamped()
base_link_tf.header.frame_id = "map"
base_link_tf.child_frame_id = "reu_1/base_link"
base_link_tf.header.stamp = rospy.Time(t_us/1000000.0)
base_link_tf.transform.translation = pos
base_link_tf.transform.rotation = quat
# publish TFs
tf_msg = tf2_msgs.msg.TFMessage()
tf_msg.transforms = []
tf_msg.transforms.append(base_link_tf)
outbag.write("/tf", tf_msg, rospy.Time(t_us/1000000.0)) 
tf_msg = tf2_msgs.msg.TFMessage()
tf_msg.transforms = []
tf_msg.transforms.append(navcam_sensor_tf)
outbag.write("/tf", tf_msg, rospy.Time(t_us/1000000.0))
# open corresponding .pcd file
pcd_path = json_data["data"]["path"]
pcl_data = pcl.load(metadata_dir + "/" + pcd_path)
# pcl_data = pypcd.(metadata_dir + "/" + pcd_path)
# get data
pcl_msg = sensor_msgs.msg.PointCloud2()
pcl_msg.data = np.ndarray.tobytes(pcl_data.to_array())
pcl_msg.header.stamp = rospy.Time(t_us/1000000.0)# t_s, t_ns)
pcl_msg.header.frame_id = "reu_1/navcam_sensor"

# Pusblish Pointcloud2 msg
outbag.write("/reu_1/pcl_navcam", pcl_msg, rospy.Time(t_us/1000000.0))
pass

if __name__ == "__main__":
dataset_path = "/home/---/Documents/datasets/---/" 
main()

base_link和camera tfs来自json文件，该文件还存储一个字符串来关联.pcd文件。

Answer 1

您发布的代码的一个问题是，它只创建一个PointCloud2消息每个文件。也就是说，已经有一个包可以实现您的期望，请查看这个pcl_ros模块。您可以创建PointCloud2消息并将其与rosrun pcl_ros pcd_to_pointcloud <file.pcd> [ <interval> ]一起发布。

同样值得注意的是:如果你正在运行一个完整的ROS桌面安装，你实际上不需要单独安装pcl库;它们被嵌入默认的ROS安装。