点云XYZ

Question

我是利用罗斯节点的安森索相机。从这里，我想读取点云数据，它以消息类型/point_cloud发布在PointCloud2主题上，并将其转换为可用于断言机器人目的地的数据。

我试过用

sensor_msgs.point_cloud2.read_points(pointCloud, 
                                    field_names=pointCloud.data, 
                                    skip_nans=True)

它实际上返回了三个值，从脚本point_cloud2.py中我了解到它们实际上是x、y和z坐标。问题是这些是值小于1的浮点数。

我还尝试直接读取pointCloud.data中的数据，但这导致原始的未排序数据无法应用。

    def point_cloud_callback(self, pc_msg):
        rospy.logdebug("Get a point cloud")
        if self._msg_lock.acquire(False): 
            self._last_point_cloud_msg = pc_msg
            self._msg_lock.release()

    def print_pc(self):
        pointCloud = self._last_point_cloud_msg

        for point in sensor_msgs.point_cloud2.read_points(pointCloud,
                            field_names=pointCloud.data, 
                            skip_nans=True):
            pt_x = point[0]
            pt_y = point[1]
            pt_z = point[2]
            print pt_x, pt_y, pt_z

def main():
    rospy.init_node("simple_pc_subscriber")
    node = pc_subscriber()
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        node.print_pc()
        rate.sleep()

if __name__ == '__main__':
    main()

我所得到的结果是浮点数在0到1之间，我期望得到基于像素的坐标。相机的分辨率是1280 * 1080，这是我期望的x和y值，对于每一个像素，一个表示Z坐标的深度值，可以用来确定物体的位置。

Answer 1

我希望你做得很好。你应该把你的数据映射到你想要的分辨率。您可以这样做:x= int(X_1280) y= int(Y_1080)

其中X和Y是它的输出：

    sensor_msgs.point_cloud2.read_points(pointCloud, 
                                field_names=pointCloud.data, 
                                skip_nans=True)

Z以0，1表示强度，你也可以把它乘以255。