def save_bag(range_imgs):
bag = rosbag.Bag('test.bag', 'w')
bag_topic = '/object_scan/raw_cloud'
for i in range(0, range_imgs.shape[0]):
clouds = range2xyz(range_imgs[i])
cloud_msg = pc2.create_cloud_xyz32(
header,
clouds.reshape(-1, 3).tolist(
)) #TODO unsure about the order when I convert 64x1024 to 63000
bag.write(bag_topic, cloud_msg)
def add_prd(path, range_imgs):
input_bag = rosbag.Bag(path)
topics, types = get_topics_types(input_bag)
output_bag = rosbag.Bag('test.bag', 'w')
bag_topic = '/object_scan/raw_cloud'
count = 0
for topic, msg, t in input_bag.read_messages(topics=[
bag_topic,
]):
cloud = range2xyz(range_imgs[count])
cloud_binary = convert_binary(cloud, msg.height, msg.width)
cloud_msg = create_pc2_msg(msg.header, msg.height, msg.width, fields,
16, cloud_binary) #clouds.reshape(-1,3))
# cloud_msg = pc2.create_cloud_xyz32(msg.header, clouds.reshape(-1,3).tolist()) #TODO unsure about the order when I convert 64x1024 to 63000
output_bag.write(bag_topic, cloud_msg)
count += 1
progressbar(count, input_bag.get_message_count() / len(topics))
#from sensor_msgs import point_cloud2
#from sensor_msgs.msg import PointCloud2, PointField
#from std_msgs.msg import Header
from numpy2pcd import get_low_res_input, noise_remove, range2xyz, range2pcd
from readpcd import offset_range_img, image_rows_full, image_cols, save_grey_img, create_4dir, progressbar, reverse_offset_range_img
range_image_array = np.empty([0, image_rows_full, image_cols, 1],
dtype=np.float32)
file_path = '/home/yifu/data/long_experiment/cloud_0000_1583840211_539847424.pcd'
pc = pypcd.PointCloud.from_path(file_path)
pcdata = pc.pc_data.view(np.float32).reshape(-1, 3)
np.savetxt("/home/yifu/data/long_experiment/processed pcd/raw %s.csv" %
sys.version[:3],
pcdata,
delimiter=",")
range_image = offset_range_img(pcdata)
# recover
cloud = range2xyz(range_image[0])
cloud = reverse_offset_range_img(cloud)
np.savetxt("/home/yifu/data/long_experiment/processed pcd/%s.csv" %
sys.version[:3],
cloud,
delimiter=",")
output_pcd = pypcd.make_xyz_point_cloud(cloud.reshape(-1, 3))
output_pcd.save(
join('/home/yifu/data/long_experiment/processed pcd/numpysave.pcd'))
count=1))
depth_points.fields.append(
sensor_msgs.msg.PointField(name="y",
offset=4,
datatype=sensor_msgs.msg.PointField.FLOAT32,
count=1))
depth_points.fields.append(
sensor_msgs.msg.PointField(name="z",
offset=8,
datatype=sensor_msgs.msg.PointField.FLOAT32,
count=1))
depth_points.point_step = 16
depth_points.row_step = depth_points.point_step * depth_points.width
buffer = []
buffer_rgb = []
range_img = range2xyz(gt[count])
for v in range(depth_image.height):
for u in range(depth_image.width):
ptx = range_img[v, u, 0]
pty = range_img[v, u, 1]
ptz = range_img[v, u, 2]
buffer.append(struct.pack('ffff', ptx, pty, ptz, 1.0))
depth_points.data = "".join(buffer)
outbag.write("/lidarsr/prediction", depth_points, t)
count += 1
if count > 3:
break
outbag.close()