I want to create a simple python script to read some .pcd files and create a sensor_msgs:ointCloud2 for each in a rosbag. I tried using the python-pcl library, but I'm probably doing something wrong when adding the points to the data field, because when playing the rosbag and checking with RViz and echoing the topic I get no points. This is the part where I set the PointCloud2 msg. pcl_data = pcl.load(metadata_dir + "/" + pcd_path) # get data pcl_msg = sensor_msgs.msg.PointCloud2() pcl_msg.data = np.ndarray.tobytes(pcl_data.to_array()) pcl_msg.header.stamp = rospy.Time(t_us/10000000.0) pcl_msg.header.frame_id = "robot_1/navcam_sensor" # Pusblish Pointcloud2 msg outbag.write("/robot_1/pcl_navcam", pcl_msg, rospy.Time(t_us/10000000.0)) I also tried pypc without any luck as well. How would you do it? Maybe there is a ToROSMsg method somewhere like in the cpp version of pcl? Is there a python equivalent for what is very easily available in cpp: pcl::toROSMsg ? Thank you Here is the full code of the python script: #! /usr/bin/env python3 import rospy import rosbag import tf2_msgs.msg import geometry_msgs.msg import sensor_msgs.msg import sys import os import json import numpy as np import tf.transformations as tf_transformations import pcl import json import math import pypcd import sensor_msgs.point_cloud2 as pc2 import tf2_msgs.msg._TFMessage def main(): output_bag_path = dataset_path + "rosbag.bag" with rosbag.Bag(output_bag_path, 'w') as outbag: # iterate metadata files with tfs metadata_dir = dataset_path + "Pointcloud/metadata" t_first_flag = False # for filename in os.listdir(metadata_dir): list_of_files = sorted( filter( lambda x: os.path.isfile(os.path.join(metadata_dir, x)), os.listdir(metadata_dir) ) ) for filename in list_of_files: # open json file json_path = os.path.join(metadata_dir, filename) json_file = open(json_path) json_data = json.load(json_file) # get timestamp t_us = json_data \ ["metadata"] \ ["Timestamps"] \ ["microsec"] t_ns, t_s = math.modf(t_us/1000000) # get camera tf pos = geometry_msgs.msg.Vector3( \ json_data["metadata"] \ ["pose_robotFrame_sensorFrame"] \ ["data"] \ ["translation"][0], \ json_data["metadata"] \ ["pose_robotFrame_sensorFrame"] \ ["data"] \ ["translation"][1], \ json_data["metadata"] \ ["pose_robotFrame_sensorFrame"] \ ["data"] \ ["translation"][2]) quat = geometry_msgs.msg.Quaternion( \ json_data["metadata"] \ ["pose_robotFrame_sensorFrame"] \ ["data"] \ ["orientation"] \ ["x"], \ json_data["metadata"] \ ["pose_robotFrame_sensorFrame"] \ ["data"] \ ["orientation"] \ ["y"], \ json_data["metadata"] \ ["pose_robotFrame_sensorFrame"] \ ["data"] \ ["orientation"] \ ["z"], \ json_data["metadata"] \ ["pose_robotFrame_sensorFrame"] \ ["data"] \ ["orientation"] \ ["w"], ) navcam_sensor_tf = geometry_msgs.msg.TransformStamped() navcam_sensor_tf.header.frame_id = "reu_1/base_link" navcam_sensor_tf.child_frame_id = "reu_1/navcam_sensor" navcam_sensor_tf.header.stamp = rospy.Time(t_us/1000000.0) navcam_sensor_tf.transform.translation = pos navcam_sensor_tf.transform.rotation = quat # get base_link tf pos = geometry_msgs.msg.Vector3( \ json_data["metadata"] \ ["pose_fixedFrame_robotFrame"] \ ["data"] \ ["translation"][0], \ json_data["metadata"] \ ["pose_fixedFrame_robotFrame"] \ ["data"] \ ["translation"][1], \ json_data["metadata"] \ ["pose_fixedFrame_robotFrame"] \ ["data"] \ ["translation"][2]) quat = geometry_msgs.msg.Quaternion( \ json_data["metadata"] \ ["pose_fixedFrame_robotFrame"] \ ["data"] \ ["orientation"] \ ["x"], \ json_data["metadata"] \ ["pose_fixedFrame_robotFrame"] \ ["data"] \ ["orientation"] \ ["y"], \ json_data["metadata"] \ ["pose_fixedFrame_robotFrame"] \ ["data"] \ ["orientation"] \ ["z"], \ json_data["metadata"] \ ["pose_fixedFrame_robotFrame"] \ ["data"] \ ["orientation"] \ ["w"], ) base_link_tf = geometry_msgs.msg.TransformStamped() base_link_tf.header.frame_id = "map" base_link_tf.child_frame_id = "reu_1/base_link" base_link_tf.header.stamp = rospy.Time(t_us/1000000.0) base_link_tf.transform.translation = pos base_link_tf.transform.rotation = quat # publish TFs tf_msg = tf2_msgs.msg.TFMessage() tf_msg.transforms = [] tf_msg.transforms.append(base_link_tf) outbag.write("/tf", tf_msg, rospy.Time(t_us/1000000.0)) tf_msg = tf2_msgs.msg.TFMessage() tf_msg.transforms = [] tf_msg.transforms.append(navcam_sensor_tf) outbag.write("/tf", tf_msg, rospy.Time(t_us/1000000.0)) # open corresponding .pcd file pcd_path = json_data["data"]["path"] pcl_data = pcl.load(metadata_dir + "/" + pcd_path) # pcl_data = pypcd.(metadata_dir + "/" + pcd_path) # get data pcl_msg = sensor_msgs.msg.PointCloud2() pcl_msg.data = np.ndarray.tobytes(pcl_data.to_array()) pcl_msg.header.stamp = rospy.Time(t_us/1000000.0)# t_s, t_ns) pcl_msg.header.frame_id = "reu_1/navcam_sensor" # Pusblish Pointcloud2 msg outbag.write("/reu_1/pcl_navcam", pcl_msg, rospy.Time(t_us/1000000.0)) pass if __name__ == "__main__": dataset_path = "/home/---/Documents/datasets/---/" main() The base_link and camera tfs come from a json file that also stores a string to associate the .pcd file. Continue reading...
