This example shows how to write intensity data points to a ROS or ROS 2 PointCloud2 message structure. To write intensity data points to a ROS or ROS 2 PointCloud2 message, you must write x,y and z data points first.
Create a random m
-by-n
-by-3
matrix with x, y and z coordinate points.
Create a sensor_msgs/PointCloud2
message in ROS network.
rosMsg = struct with fields:
MessageType: 'sensor_msgs/PointCloud2'
Header: [1×1 struct]
Height: 0
Width: 0
Fields: [0×1 struct]
IsBigendian: 0
PointStep: 0
RowStep: 0
Data: [0×1 uint8]
IsDense: 0
Write the x, y and z coordinate points to the ROS message and set PointStep
of sensor_msgs/PointCloud2
to 16 to store the intensity data points.
rosMsg = struct with fields:
MessageType: 'sensor_msgs/PointCloud2'
Header: [1×1 struct]
Height: 480
Width: 640
Fields: [3×1 struct]
IsBigendian: 0
PointStep: 16
RowStep: 10240
Data: [4915200×1 uint8]
IsDense: 1
Create a random m
-by-n
matrix with intensity values.
Write the intensity information to the ROS message and set the offset of the intensity field in sensor_msgs/PointField
to 8. This means that intensity field begins to be stored from 16th byte for each PointStep
.
rosMsg = struct with fields:
MessageType: 'sensor_msgs/PointCloud2'
Header: [1×1 struct]
Height: 480
Width: 640
Fields: [4×1 struct]
IsBigendian: 0
PointStep: 16
RowStep: 10240
Data: [4915200×1 uint8]
IsDense: 1
You can also create a sensor_msgs/PointCloud2
message in ROS 2 network.
Write the x, y and z coordinate points to the ROS 2 message. Set PointStep
to 16.
ros2Msg = struct with fields:
MessageType: 'sensor_msgs/PointCloud2'
header: [1×1 struct]
height: 480
width: 640
fields: [3×1 struct]
is_bigendian: 0
point_step: 16
row_step: 10240
data: [4915200×1 uint8]
is_dense: 1
Write the intensity information to the ROS 2 message and set FieldOffset
to 8.
ros2Msg = struct with fields:
MessageType: 'sensor_msgs/PointCloud2'
header: [1×1 struct]
height: 480
width: 640
fields: [4×1 struct]
is_bigendian: 0
point_step: 16
row_step: 10240
data: [4915200×1 uint8]
is_dense: 1