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:
- “rosmessage” function: https://www.mathworks.com/help/ros/ref/rosmessage.html
- “rosbagwriter” function: https://www.mathworks.com/help/ros/ref/rosbagwriter.html
- Resource to understand the PointCloud2 message: https://medium.com/@tonyjacob_/pointcloud2-message-explained-853bd9907743
Hope you find the above explanation and suggestions useful!
