将点云从.pcd读取到ROS PointCloud2

Question

我想创建一个简单的Python脚本来读取一些.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 但没有任何运气。

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

cpp 中是否有一个相当于 Python 的等价物：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 和相机 tfs 来自一个 json 文件，该文件还存储一个字符串来关联 .pcd 文件。

Answer 1

您发布的代码的一个问题是它仅创建一条 PointCloud2 消息每个文件。话虽这么说，已经有一个包可以完成您所希望的事情，查看这个 pcl_ros 模块。您可以创建 PointCloud2 消息并使用

 rosrun pcl_ros pcd_to_pointcloud <file.pcd> [ <interval> ]

发布它。

另请注意：如果您正在运行完整的 ROS 桌面安装，则实际上不需要单独安装 pcl 库；它们被纳入默认的 ROS 安装中。