XYZ from pointcloud

Question

I am utilizing the ros node for the ensenso camera. From this I want to read the pointcloud data, which is published on the /point_cloud topic with the message type PointCloud2, and convert this to data that can be used to assert a destination for a robot.

I have tried using

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

which does infact return three values, from the script point_cloud2.py I understand that these are infact the x, y and z coordinates. The issue is that these are floats with values below 1.

I also tried reading the data in pointCloud.data directly but this resulted in raw unsorted data that I will not be able to apply.

    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()

The results that I am getting are floats with values between 0 and 1. I am expecting to get coordinates based on the pixels they coincide with. Resolution of the camera is 1280 * 1080, which i expect as the x and y values, and for every pixel a depth value representing the Z coordinate that can be used to ascertain the position of an object.

Answer 1

I hope you're doing well. You should map your data to the resolution you want. You can do it like this: x = int(X1280) y = int(Y1080)

where X and Y are the output of this:

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

The Z shows the intensity in [0, 1]. You can also multiply it by 255.