Transform point cloud to the world frame

this is my first question so please forgive me if I write anything wrong. My problem is as follows: I spawned a drone in Gazebo and wrote a Python code to save the point cloud I received from the drone as a pcd whenever I wanted. I tried to make a frame transform, I don’t get any errors, but my code does not work correctly. When I upload the pcds to Cloud Compare, I can see that it is wrong. The frame is set to camera_depth_optical_frame according to the drone, how can I transform it to world frame?

There must be an error in the Transform point cloud to the world frame section. This is my code:

import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import pcl
import ros_numpy
import numpy as np
import tf2_ros
from tf2_sensor_msgs.tf2_sensor_msgs import do_transform_cloud
import tf2_geometry_msgs.tf2_geometry_msgs

def write_file(c):
    with open("/home/ubuntu/catkin_ws/src/my_package/counter.txt", "w") as d:
        d.write(str(c))

def read_file():
    with open("/home/ubuntu/catkin_ws/src/my_package/counter.txt", "r") as d:
        counter = d.read()
    return int(counter)

class PointCloudRecorder:
    def __init__(self):
        print("PCR başlatıldı...")
        self.tf_buffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tf_buffer)
        self.point_cloud_subscriber = rospy.Subscriber("/camera/depth/points", PointCloud2, self.point_cloud_callback)
        self.counter = read_file()

    def point_cloud_callback(self, msg):
        print("Point cloud data received...")
        choice = input("Get data? (Y/N) ")

        if choice.upper() == 'Y':
            try:
                # Transform point cloud to the world frame
                transform = self.tf_buffer.lookup_transform('world', msg.header.frame_id, rospy.Time(0), rospy.Duration(1.0))
                transformed_cloud = do_transform_cloud(msg, transform)

                # Convert the transformed point cloud to PCL format
                pc_array = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(transformed_cloud)
                pc_array = pc_array.astype(np.float32)
                cloud = pcl.PointCloud(np.array(pc_array, dtype=np.float32))

                # Save the point cloud
                file_name = f"point_cloud_{self.counter:04d}.pcd"
                file_path = f"/home/ubuntu/catkin_ws/src/my_package/src/cc_service/{file_name}"
                pcl.save(cloud, file_path)
                rospy.loginfo(f"Point cloud saved to: {file_path}")

                self.counter += 1
                write_file(self.counter)
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
                rospy.logerr(f'Transform error: {str(e)}')
        else:
            print("Data collection skipped.")

if __name__ == "_main_":
    rospy.init_node('point_cloud_recorder')
    recorder = PointCloudRecorder()
    rospy.spin()

New contributor

Cc3 is a new contributor to this site. Take care in asking for clarification, commenting, and answering.
Check out our Code of Conduct.

Trang chủ Giới thiệu Sinh nhật bé trai Sinh nhật bé gái Tổ chức sự kiện Biểu diễn giải trí Dịch vụ khác Trang trí tiệc cưới Tổ chức khai trương Tư vấn dịch vụ Thư viện ảnh Tin tức - sự kiện Liên hệ Chú hề sinh nhật Trang trí YEAR END PARTY công ty Trang trí tất niên cuối năm Trang trí tất niên xu hướng mới nhất Trang trí sinh nhật bé trai Hải Đăng Trang trí sinh nhật bé Khánh Vân Trang trí sinh nhật Bích Ngân Trang trí sinh nhật bé Thanh Trang Thuê ông già Noel phát quà Biểu diễn xiếc khỉ Xiếc quay đĩa Dịch vụ tổ chức sự kiện 5 sao Thông tin về chúng tôi Dịch vụ sinh nhật bé trai Dịch vụ sinh nhật bé gái Sự kiện trọn gói Các tiết mục giải trí Dịch vụ bổ trợ Tiệc cưới sang trọng Dịch vụ khai trương Tư vấn tổ chức sự kiện Hình ảnh sự kiện Cập nhật tin tức Liên hệ ngay Thuê chú hề chuyên nghiệp Tiệc tất niên cho công ty Trang trí tiệc cuối năm Tiệc tất niên độc đáo Sinh nhật bé Hải Đăng Sinh nhật đáng yêu bé Khánh Vân Sinh nhật sang trọng Bích Ngân Tiệc sinh nhật bé Thanh Trang Dịch vụ ông già Noel Xiếc thú vui nhộn Biểu diễn xiếc quay đĩa Dịch vụ tổ chức tiệc uy tín Khám phá dịch vụ của chúng tôi Tiệc sinh nhật cho bé trai Trang trí tiệc cho bé gái Gói sự kiện chuyên nghiệp Chương trình giải trí hấp dẫn Dịch vụ hỗ trợ sự kiện Trang trí tiệc cưới đẹp Khởi đầu thành công với khai trương Chuyên gia tư vấn sự kiện Xem ảnh các sự kiện đẹp Tin mới về sự kiện Kết nối với đội ngũ chuyên gia Chú hề vui nhộn cho tiệc sinh nhật Ý tưởng tiệc cuối năm Tất niên độc đáo Trang trí tiệc hiện đại Tổ chức sinh nhật cho Hải Đăng Sinh nhật độc quyền Khánh Vân Phong cách tiệc Bích Ngân Trang trí tiệc bé Thanh Trang Thuê dịch vụ ông già Noel chuyên nghiệp Xem xiếc khỉ đặc sắc Xiếc quay đĩa thú vị
Trang chủ Giới thiệu Sinh nhật bé trai Sinh nhật bé gái Tổ chức sự kiện Biểu diễn giải trí Dịch vụ khác Trang trí tiệc cưới Tổ chức khai trương Tư vấn dịch vụ Thư viện ảnh Tin tức - sự kiện Liên hệ Chú hề sinh nhật Trang trí YEAR END PARTY công ty Trang trí tất niên cuối năm Trang trí tất niên xu hướng mới nhất Trang trí sinh nhật bé trai Hải Đăng Trang trí sinh nhật bé Khánh Vân Trang trí sinh nhật Bích Ngân Trang trí sinh nhật bé Thanh Trang Thuê ông già Noel phát quà Biểu diễn xiếc khỉ Xiếc quay đĩa
Thiết kế website Thiết kế website Thiết kế website Cách kháng tài khoản quảng cáo Mua bán Fanpage Facebook Dịch vụ SEO Tổ chức sinh nhật