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()
Cc3 is a new contributor to this site. Take care in asking for clarification, commenting, and answering.
Check out our Code of Conduct.