Add field to pointcloud 2

Hello!

I am trying to modify an existing pointcloud2 by adding a new field to the pointcloud. This new field will be filled with zeros, except when a condition is met (for this example, i just placed when the field azimuth of a point is greater then 10), which should change the value of the new field to 20.

How do I write this new field to my bag file? I am not being able to modify the pointcloud_msg object.

bag_file = 'path_to_my_bag_file.bag"

bag = rosbag(bag_file);

pointcloud_msg = select(bag, 'Topic', '/my_pointcloudtopic');

msgs = readMessages(pointcloud_msg);

for message_number = 1:numel(msgs) %loop over all messages of the desired topic

msg = msgs{message_number};

num_points = msg.Width; %number of points in each message

vector_zeros = single(zeros(num_points, 1)); % Creates a vector with x elements, initialized to zero

all_azimuth_values_of_msg = readField(msg,'az');%Retrieve the azimuth coordinates of all points

for point = 1:num_points %read point by point

pt_az = all_azimuth_values_of_msg(point);

if pt_az > 10

vector_zeros(point) = 20;

end

%HERE I WANT TO APPEND THE NEW FIELD TO THE POINTCLOUD AND AFTER THE LOOP WRITE IT TO A NEW BAG FILE

end

end

Answer 1

[Garmit Pant]

Hello Pedro

From what I gather, you have a “rosbag” file and you want to read a message from it. You're modifying a “PointCloud2” message in the file by adding a new field based on azimuth values and seek to write the updated message to a new bag file.

Your approach to read the messages from the file using MATLAB function “readMessages” is correct. But the approach followed to read point-by-point is incorrect. Depending on the data, the “Data” in a “PointCloud2” message is stored as a 1D array of size (total number of points)*( length of a point in bytes). The graphic below shows how the data is stored:

You need to check the information about the “PointStep” and existing fields, which can be done using:

msg

This will display the information about the message. Below is an example output:

If the “Fields” have datatype as “uint8”, your approach to read the field data should work.

A new field can be created using the MATLAB function “rosmessage” as follows:

% Create a new PointField 
newField = rosmessage('sensor_msgs/PointField'); 

% Configure the new PointField 
newField.Name = 'intensity'; % Example field name 
newField.Offset = <desired_offset>; % Offset in bytes to the start of the first element in the point cloud data 
newField.Datatype = 1; % Datatype (1 = UINT8) 
newField.Count = 1; % Number of elements in the field (1 = one intensity value per point) 

After creating the vector containing the values of the new field according to your criterion, you need to replace the “Data” of the message with a new 1D array such that the new array takes into account the addition of the new field. The data point of the new field will be added at the set offset, typically after adding the pre-existing fields.

Since a new field is added, you’ll also need to adjust the “PointStep” and “RowStep” need to be adjusted accordingly.

This new message can be written to a new file using the MATLAB function “rosbagwriter”. Following are placeholder commands that can be used.

writer = rosbagwriter(newBagFilePath); 
write(writer, '/my_pointcloudtopic', msgStruct, pointcloud_msgs.MessageList(message_number).Time); 

By following the steps above and adapting them according to your data, you’ll be able to add a new field to your “PointCloud2” message and write the messages to a new “rosbag” file.

For further understanding, I suggest you refer to the following MathWorks Documentation and resources:

1. “rosmessage” function: https://www.mathworks.com/help/ros/ref/rosmessage.html
2. “rosbagwriter” function: https://www.mathworks.com/help/ros/ref/rosbagwriter.html
3. Resource to understand the PointCloud2 message: https://medium.com/@tonyjacob_/pointcloud2-message-explained-853bd9907743

Hope you find the above explanation and suggestions useful!