![](/img/trans.png)
[英]how to effeciently convert ROS PointCloud2 to pcl point cloud and visualize it in python
[英]Reading Pointcloud from .pcd to ROS PointCloud2
我想創建一個簡單的 python 腳本來讀取 some.pcd 文件並為 rosbag 中的每個文件創建一個 sensor_msgs::PointCloud2。
我嘗試使用 python-pcl 庫,但在將點添加到數據字段時我可能做錯了什么,因為在播放 rosbag 並使用 RViz 檢查並回應主題時我沒有得到任何分數。
這是我設置 PointCloud2 消息的部分。
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 也沒有任何運氣。
你會怎么做? 也許在某個地方有一個 ToROSMsg 方法,比如在 pcl 的 cpp 版本中?
是否有一個 python 等效於在 cpp 中很容易獲得的東西:pcl::toROSMsg?
謝謝
以下是 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 文件。
您發布的代碼的一個問題是它只為每個文件創建一個 PointCloud2 消息。 話雖這么說,已經有一個 package 可以做你希望做的事情,看看這個 pcl_ros 模塊。 您可以創建 PointCloud2 消息並使用rosrun pcl_ros pcd_to_pointcloud <file.pcd> [ <interval> ]
發布它。
另外請注意:如果您正在運行完整的 ROS 桌面安裝,您實際上不需要單獨安裝 pcl 庫; 它們被嵌入到默認的 ROS 安裝中。
聲明:本站的技術帖子網頁,遵循CC BY-SA 4.0協議,如果您需要轉載,請注明本站網址或者原文地址。任何問題請咨詢:yoyou2525@163.com.