0
$\begingroup$

I am trying to filter out white points from the ZED2i published pointcloud and I am encountering a lot of issues.

header:
  stamp:
    sec: 1707101581
    nanosec: 803938020
  frame_id: zed_left_camera_frame
height: 360
width: 640
fields:
 - name: x
  offset: 0
  datatype: 7
  count: 1
 - name: y
  offset: 4
  datatype: 7
  count: 1
 - name: z
  offset: 8
  datatype: 7
  count: 1
 - name: rgb
  offset: 12
  datatype: 7
  count: 1
is_bigendian: false
point_step: 16
row_step: 10240
data:
 - 255
 - 255
 - 255
 - 127
 - 255
 - 255
 - 255
 - 127
 - 255
 - 255
 - 255
 - 127
 - 255
 - 255
 - 255
 - 127
 - 255
 - 255
 - 255
 - 127
 - 255
 - 255
 - 255
 - 127
 - 255
 - 255
 - 255
 - 127
 - 255
 - 255
 - 255
 - 127

This is what I get from the ros2 topic echo that the zed 2i camera publishes, I created a custom code, where I am trying to create a condition based on the data field so that it omits points with rgb values below 200, by creating a new pointcloud, ‘filtered_points’ that is the exact same as the original, but with only the desired values. However, it is publishing no points or messages on rviz2.

ZED2i - each point of the point cloud is a data composed of 4 32bit floating point values in the format XYZBGRA.

I think I am not converting it correctly

If you have any suggestions to approach this problem it would be greatly appreciated.

Custom Code:


import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2
import numpy as np

class pc(Node):
    def __init__(self):
        super().__init__("pc")
        self.pc_org_sub = self.create_subscription(
            PointCloud2, "/zed/zed_node/point_cloud/cloud_registered",
            self.pc_callback, 10)
        self.pc_new_pub = self.create_publisher(
            PointCloud2, "/PointCloud2_filtered", 10)
        self.get_logger().info("Node has been started")

    # Subscriber
    def pc_callback(self, pc: PointCloud2):
        # Assuming RGB fields are at offset 12, 13, 14 and have datatype uint8
        rgb_offset = 12
        rgb_values = np.frombuffer(pc.data, dtype=np.uint8)[rgb_offset::pc.point_step]

        # Check if all RGB values are above 200
        if np.all(rgb_values > 200):
            # Extract all the data
            desirable_data = pc.data

            # Create a new PointCloud2 message with all the data
            filtered_pc = PointCloud2()
            filtered_pc.header = pc.header
            filtered_pc.height = pc.height
            filtered_pc.width = pc.width
            filtered_pc.fields = pc.fields
            filtered_pc.is_bigendian = pc.is_bigendian
            filtered_pc.point_step = pc.point_step
            filtered_pc.row_step = pc.row_step
            filtered_pc.data = desirable_data

            # Publish the new PointCloud2 message
            self.pc_new_pub.publish(filtered_pc)

        else:
            return


def main(args=None):
    rclpy.init(args=args)
    node = pc()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
$\endgroup$
1
  • $\begingroup$ Please format the code in your question correctly $\endgroup$ Feb 6 at 1:08

1 Answer 1

0
$\begingroup$

I'm not experienced in working with PointClouds, but once I gave a try to the module PointCloud2 from sensors_msgs. You can access to it using the next line import sensor_msgs.point_cloud2. There you have some functions like read_points and create_cloud. I attach the API reference https://docs.ros.org/en/noetic/api/sensor_msgs/html/namespacesensor__msgs_1_1point__cloud2.html#a5d969a10974e5f12199d0da5a7f3ec92. Perhaps you could give it a try and see if it performs better! I don't know if it could be related to ordered pointclouds, if you are removing points from the cloud, you perhaps need to resize the height and width fields?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.