Hello,
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.
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()