Work with Specialized ROS Messages
This example shows how to handle message types for laser scans, uncompressed and compressed images, point clouds, camera info, occcupancy grid, and octomap messages.
These are some commonly used ROS messages stored data in a format, that requires some transformation before it can be used for further processing. MATLAB® can help you by formatting these specialized ROS messages for easy use.
Prerequisites: Work with Basic ROS Messages
Load Sample Messages
Load some sample messages. These messages are populated with real and synthetic data from various robotics sensors.
load("specialROSMessageData.mat")
Image Messages
MATLAB provides support for image messages, which always have the message type sensor_msgs/Image
.
Create an empty image message using rosmessage
to see the standard ROS format for an image message.
emptyimg = rosmessage("sensor_msgs/Image",DataFormat="struct")
emptyimg = struct with fields:
MessageType: 'sensor_msgs/Image'
Header: [1x1 struct]
Height: 0
Width: 0
Encoding: ''
IsBigendian: 0
Step: 0
Data: [0x1 uint8]
For convenience, an image message that is fully populated and is stored in the img
variable is loaded from specialROSMessageData.mat
.
Inspect the image message variable img
in your workspace. The size of the image is stored in the Width
and Height
properties. ROS sends the actual image data using a vector in the Data
property.
img
img = struct with fields:
MessageType: 'sensor_msgs/Image'
Header: [1x1 struct]
Height: 480
Width: 640
Encoding: 'rgb8'
IsBigendian: 0
Step: 1920
Data: [921600x1 uint8]
The Data
property stores raw image data that cannot be used directly for processing and visualization in MATLAB. You can use the rosReadImage
function to retrieve the image in a format that is compatible with MATLAB.
imageFormatted = rosReadImage(img);
The original image has a rgb8 encoding. By default, rosReadImage
returns the image in a standard 480-by-640-by-3 uint8
format. View this image using the imshow
function.
imshow(imageFormatted)
MATLAB® supports all ROS image encoding formats, and rosReadImage
handles the complexity of converting the image data. In addition to color images, MATLAB also supports monochromatic and depth images.
Additionally, MATLAB provides the rosWriteImage
function to convert a MATLAB image to a ROS message using the function. Apply rudimentary object detection on the sample image with color thresholding. Visualize the modified image.
greenPercentage = 100*double(imageFormatted(:,:,2))./sum(imageFormatted,3); thresholdImg = 255*uint8(greenPercentage > 35); imshow(thresholdImg)
Write the modified image to a ROS message with the rosWriteImage
function. Since the modified image only has 1 channel and is of type uint8
, use the mono8
encoding.
imageMsg = rosWriteImage(emptyimg,thresholdImg,Encoding="mono8");
Compressed Image Messages
Many ROS systems send their image data in a compressed format. MATLAB provides support for these compressed image messages.
Create an empty compressed image message using rosmessage
. Compressed images in ROS have the message type sensor_msgs/CompressedImage
and have a standard structure.
emptyimgcomp = rosmessage("sensor_msgs/CompressedImage",DataFormat="struct")
emptyimgcomp = struct with fields:
MessageType: 'sensor_msgs/CompressedImage'
Header: [1x1 struct]
Format: ''
Data: [0x1 uint8]
For convenience, a compressed image message that is already populated was loaded from specialROSMessageData.mat
.
Inspect the imgcomp
variable that was captured by a camera. The Format
property captures all the information that MATLAB needs to decompress the image data stored in Data
.
imgcomp
imgcomp = struct with fields:
MessageType: 'sensor_msgs/CompressedImage'
Header: [1x1 struct]
Format: 'bgr8; jpeg compressed bgr8'
Data: [30376x1 uint8]
Similar to the image message, you can use rosReadImage
to obtain the image in standard RGB format. Even though the original encoding for this compressed image is bgr8
, rosReadImage
does the conversion.
compressedFormatted = rosReadImage(imgcomp);
Visualize the image using the imshow
function.
imshow(compressedFormatted)
Most image formats are supported for the compressed image message type. The 16UC1
and 32FC1
encodings are not supported for compressed depth images. Monochromatic and color image encodings are supported.
Point Cloud Messages
Point clouds can be captured by a variety of sensors used in robotics, including LIDARs, Kinect®, and stereo cameras. The most common message type in ROS for transmitting point clouds is sensor_msgs/PointCloud2
and MATLAB provides some specialized functions for you to work with this data.
You can see the standard ROS format for a point cloud message by creating an empty point cloud message.
emptyptcloud = rosmessage("sensor_msgs/PointCloud2",DataFormat="struct")
emptyptcloud = struct with fields:
MessageType: 'sensor_msgs/PointCloud2'
Header: [1x1 struct]
Height: 0
Width: 0
Fields: [0x1 struct]
IsBigendian: 0
PointStep: 0
RowStep: 0
Data: [0x1 uint8]
IsDense: 0
View the populated point cloud message that is stored in the ptcloud
variable in your workspace:
ptcloud
ptcloud = struct with fields:
MessageType: 'sensor_msgs/PointCloud2'
Header: [1x1 struct]
Height: 480
Width: 640
Fields: [4x1 struct]
IsBigendian: 0
PointStep: 32
RowStep: 20480
Data: [9830400x1 uint8]
IsDense: 0
The point cloud information is encoded in the Data
property of the message. You can extract the x,
y,
z coordinates as an N-by-3 matrix by calling the rosReadXYZ
function.
xyz = rosReadXYZ(ptcloud)
xyz = 307200x3 single matrix
NaN NaN NaN
NaN NaN NaN
NaN NaN NaN
NaN NaN NaN
NaN NaN NaN
NaN NaN NaN
NaN NaN NaN
NaN NaN NaN
NaN NaN NaN
NaN NaN NaN
⋮
NaN
in the point cloud data indicates that some of the x,
y,
z values are not valid. This is an artifact of the Kinect® sensor, and you can safely remove all NaN
values.
xyzValid = xyz(~isnan(xyz(:,1)),:)
xyzValid = 193359x3 single matrix
0.1378 -0.6705 1.6260
0.1409 -0.6705 1.6260
0.1433 -0.6672 1.6180
0.1464 -0.6672 1.6180
0.1502 -0.6705 1.6260
0.1526 -0.6672 1.6180
0.1556 -0.6672 1.6180
0.1587 -0.6672 1.6180
0.1618 -0.6672 1.6180
0.1649 -0.6672 1.6180
⋮
Some point cloud sensors also assign RGB color values to each point in a point cloud. If these color values exist, you can retrieve them with a call to rosReadRGB
.
rgb = rosReadRGB(ptcloud)
rgb = 307200x3 single matrix
0.8392 0.7059 0.5255
0.8392 0.7059 0.5255
0.8392 0.7137 0.5333
0.8392 0.7216 0.5451
0.8431 0.7137 0.5529
0.8431 0.7098 0.5569
0.8471 0.7137 0.5569
0.8549 0.7098 0.5569
0.8588 0.7137 0.5529
0.8627 0.7137 0.5490
⋮
You can visualize the point cloud with the rosPlot
function. rosPlot
automatically extracts the x,
y,
z coordinates and the RGB color values (if they exist) and show them in a 3-D scatter plot. The rosPlot
function ignores all NaN
x,
y,
z coordinates, even if RGB values exist for that point.
rosPlot(ptcloud)
Examine all the stored fields in the point cloud message using the rosReadAllFieldNames
function. The loaded point cloud message contains four fields x
, y
, z
, and rgb
.
fieldNames = rosReadAllFieldNames(ptcloud)
fieldNames = 1x4 cell
{'x'} {'y'} {'z'} {'rgb'}
You can access the corresponding data for any field using the rosReadField
function. You must unpack the returned data manually, depending on how it is formatted. For example, the RGB image can be extracted by type casting the data to uint8
and reshaping the result. Use the result from the rosReadAllFieldNames
function for input validation.
if any(contains(fieldNames,"rgb")) rawData = typecast(rosReadField(ptcloud,"rgb"),"uint8"); tmp = reshape(permute(reshape(rawData,4,[]),[3,2,1]),ptcloud.Width,ptcloud.Height,4); pcImg = permute(tmp(:,:,[3,2,1]),[2 1 3]); imshow(pcImg) end
Calculate the intensity value of each point in the point cloud as 1/3(r+g+b)
, where r, g, and b are the RGB color values of that point, respectively.
intensity = sum(rgb,2)/3;
Write the x, y, z, RGB, and intensity values to the empty point cloud message using rosWriteXYZ
, rosWriteRGB
, and rosWriteIntensity
functions respectively. Specify a point step size of 32
.
ptcloudWithIntensity = rosWriteXYZ(emptyptcloud,xyz,PointStep=uint32(32)); ptcloudWithIntensity = rosWriteRGB(ptcloudWithIntensity,rgb); ptcloudWithIntensity = rosWriteIntensity(ptcloudWithIntensity,intensity)
ptcloudWithIntensity = struct with fields:
MessageType: 'sensor_msgs/PointCloud2'
Header: [1x1 struct]
Height: 1
Width: 307200
Fields: [5x1 struct]
IsBigendian: 0
PointStep: 32
RowStep: 9830400
Data: [9830400x1 uint8]
IsDense: 0
Octomap Messages
ROS uses Octomap messages to implement 3D occupancy grids. Octomap messages are commonly used in robotics applications, such as 3D navigation. You can see the standard ROS format for an octomap message by creating an empty message of the appropriate type.
Use rosmessage
to create the message.
emptyoctomap = rosmessage("octomap_msgs/Octomap",DataFormat="struct")
emptyoctomap = struct with fields:
MessageType: 'octomap_msgs/Octomap'
Header: [1x1 struct]
Binary: 0
Id: ''
Resolution: 0
Data: [0x1 int8]
For convenience, an octomap
message that is fully populated and is stored in the octomap
variable loaded from specialROSMessageData.mat.
Inspect the variable octomap
in your workspace. The Data
field contains the octomap structure in a serialized format.
octomap
octomap = struct with fields:
MessageType: 'octomap_msgs/Octomap'
Header: [1x1 struct]
Binary: 1
Id: 'OcTree'
Resolution: 0.0250
Data: [3926x1 int8]
Create an occupancyMap3D
(Navigation Toolbox) object from the ROS message using the rosReadOccupancyMap3D
function. Display the 3D occupancy map using the show
function.
occupancyMap3DObj = rosReadOccupancyMap3D(octomap); show(occupancyMap3DObj)
Quaternion Messages
Quaternions are commonly used in robotics to express orientation. Use rosmessage
to create a quaternion message and observe the fields.
emptyquatmsg = rosmessage("geometry_msgs/Quaternion",DataFormat="struct")
emptyquatmsg = struct with fields:
MessageType: 'geometry_msgs/Quaternion'
X: 0
Y: 0
Z: 0
W: 0
For convenience, a quaternion message that represents a 90 degree rotation about the z-axis was loaded from specialROSMessageData.mat
. Inspect the variable quatMsg
in your workspace.
quatmsg
quatmsg = struct with fields:
MessageType: 'geometry_msgs/Quaternion'
X: 0
Y: 0
Z: 0.7071
W: 0.7071
Create a quaternion
object from a ROS message using the rosReadQuaternion
function. The quaternion
object contains the x, y, z, and w components and provides additional functionalities, such as rotating a point.
quat = rosReadQuaternion(quatmsg);
Define a point in three-dimensional space and rotate it using rotatepoint
function. Visualize the two points
cartesianPoint = [1,0,1]; plot3(cartesianPoint(1),cartesianPoint(2),cartesianPoint(3),"bo") hold on plot3([0;cartesianPoint(1)],[0;cartesianPoint(2)],[0;cartesianPoint(3)],"k") rotationResult = rotatepoint(quat,cartesianPoint); plot3(rotationResult(1),rotationResult(2),rotationResult(3),"ro") plot3([0;rotationResult(1)],[0;rotationResult(2)],[0;rotationResult(3)],"k") xlabel("x") ylabel("y") zlabel("z") grid on
Camera Info Messages
Camera calibration is a commonly used procedure in robotics vision applications. ROS provides sensor_msgs/CameraInfo
message type to publish calibration information. Use rosmessage
to create a camera info message and observe the fields.
emptycamerainfomsg = rosmessage("sensor_msgs/CameraInfo",DataFormat="struct")
emptycamerainfomsg = struct with fields:
MessageType: 'sensor_msgs/CameraInfo'
Header: [1x1 struct]
Height: 0
Width: 0
DistortionModel: ''
D: [0x1 double]
K: [9x1 double]
R: [9x1 double]
P: [12x1 double]
BinningX: 0
BinningY: 0
Roi: [1x1 struct]
Notably, the message stores the matrices K
and P
as vectors. ROS requires these matrices to be stored in row-major format. MATLAB stores matrices in column-major, hence extracting the K
and P
matrices requires reshaping and transposing.
The estimateCameraParameters
(Computer Vision Toolbox) function can be used to create cameraParameters
(Computer Vision Toolbox) and stereoParameters
(Computer Vision Toolbox) objects. You can create sensor_msgs/CameraInfo
messages from these objects using the rosWriteCameraInfo
function. The objects must be converted to structures before use. Load the camera calibration structures.
load("calibrationStructs.mat")
For convenience, the variable params
loaded from calibrationStructs.mat
is a fully populated cameraParameters
struct. Write the cameraParameters
struct to a new ROS message using the rosWriteCameraInfo
function.
msg = rosWriteCameraInfo(emptycamerainfomsg,params);
The following table shows the correspondence between the cameraParameters object and the ROS message.
exampleHelperShowCameraParametersTable
ans=5×2 table
ROS message camera parameters
___________ ______________________
Intrinsic matrix "K" "IntrinsicMatrix"
Radial distortion "D(1:2)" "RadialDistortion"
Tangential distortion "D(3:5)" "TangentialDistortion"
Height "Height" "ImageSize(1)"
Width "Width" "ImageSize(2)"
Verify that the intrinsic matrix of the ROS message matches the intrinsic matrix of params
.
K = reshape(msg.K,3,3)'
K = 3×3
714.1885 0 563.6481
0 710.3785 355.7254
0 0 1.0000
intrinsicMatrix = params.IntrinsicMatrix'
intrinsicMatrix = 3×3
714.1885 0 563.6481
0 710.3785 355.7254
0 0 1.0000
For convenience, the variable stereoParams
loaded from calibrationStructs.mat
is a fully populated stereoParameters
struct. Write the stereoParameters
struct to two new ROS messages using the rosWriteCameraInfo
function.
[msg1,msg2] = rosWriteCameraInfo(msg,stereoParams);
The following table shows the correspondence between the stereoParameters object and the ROS message.
exampleHelperShowStereoParametersTable
ans=2×2 table
ROS message stereoParameters
____________ ___________________________
Translation of camera 2 "P(:,1:2)" "TranslationOfCamera2(1:2)"
Rotation of camera 2 "inv(R1)*R2" "RotationOfCamera2"
Verify that the camera 2 rotation matrices of the ROS message and stereoParams
match.
R1 = reshape(msg1.R,3,3)'; R2 = reshape(msg2.R,3,3)'; R = R1\R2
R = 3×3
1.0000 -0.0002 -0.0050
0.0002 1.0000 -0.0037
0.0050 0.0037 1.0000
rotationOfCamera2 = stereoParams.RotationOfCamera2
rotationOfCamera2 = 3×3
1.0000 -0.0002 -0.0050
0.0002 1.0000 -0.0037
0.0050 0.0037 1.0000
Verify that the camera 2 translation vectors of the ROS message and stereoParams
match.
P = reshape(msg2.P,4,3)'; P(1:2,end)'
ans = 1×2
-119.8720 -0.4005
translationOfCamera2 = stereoParams.TranslationOfCamera2(1:2)
translationOfCamera2 = 1×2
-119.8720 -0.4005
Laser Scan Messages
Laser scanners are commonly used sensors in robotics. ROS provides sensor_msgs/LaserScan
message type to publish laser scan messages. Use rosmessage
to create a laser scan message and observe the fields.
emptyscan = rosmessage("sensor_msgs/LaserScan","DataFormat","struct")
emptyscan = struct with fields:
MessageType: 'sensor_msgs/LaserScan'
Header: [1x1 struct]
AngleMin: 0
AngleMax: 0
AngleIncrement: 0
TimeIncrement: 0
ScanTime: 0
RangeMin: 0
RangeMax: 0
Ranges: [0x1 single]
Intensities: [0x1 single]
Since you created an empty message, emptyscan
does not contain any meaningful data. For convenience, a laser scan message that is fully populated and is stored in the scan
variable was loaded from specialROSMessageData.mat
.
Inspect the scan
variable. The primary data in the message is in the Ranges
property. The data in Ranges
is a vector of obstacle distances recorded at small angle increments.
scan
scan = struct with fields:
MessageType: 'sensor_msgs/LaserScan'
Header: [1x1 struct]
AngleMin: -0.5467
AngleMax: 0.5467
AngleIncrement: 0.0017
TimeIncrement: 0
ScanTime: 0.0330
RangeMin: 0.4500
RangeMax: 10
Ranges: [640x1 single]
Intensities: [0x1 single]
You can get the scan angles from the ROS message using the rosReadScanAngles
function. Visualize the scan data in polar coordinates using the polarPlot
function.
angles = rosReadScanAngles(scan);
figure
polarplot(angles,scan.Ranges,LineWidth=2)
title("Laser Scan")
You can get the measured points in Cartesian coordinates using the rosReadCartesian
function.
xy = rosReadCartesian(scan);
This populates xy
with a list of [x,y]
coordinates that were calculated based on all valid range readings. Visualize the scan message using the rosPlot
function:
rosPlot(scan,"MaximumRange",5)
Create a lidarScan
object from a ROS message using the rosReadLidarScan
function. The lidarScan
object contains ranges, angles, and Cartesian points and provides additional functionalities, such as transforming the scanned points. Use the transformScan
function to rotate the scan point and visualize it using plot
.
lidarScanObj = rosReadLidarScan(scan)
lidarScanObj = lidarScan with properties: Ranges: [640x1 double] Angles: [640x1 double] Cartesian: [640x2 double] Count: 640
rotateScan = transformScan(lidarScanObj,[0,0,pi/2]); plot(rotateScan)
Occupancy Grid Messages
Occupancy grid messages are commonly used in robotics for 2D navigation applications. ROS provides nav_msgs/OccupancyGrid
message type to publish occupancy grid messages. Use rosmessage
to create an occupancy grid message and observe the fields.
emptyMap = rosmessage("nav_msgs/OccupancyGrid",DataFormat="struct")
emptyMap = struct with fields:
MessageType: 'nav_msgs/OccupancyGrid'
Header: [1x1 struct]
Info: [1x1 struct]
Data: [0x1 int8]
Notice that emptyMap
does not contain any meaningful data. For convenience, an occupancy grid message that is fully populated and is stored in the mapMsg
variable loaded from specialROSMessageData.mat.
Inspect the mapMsg
variables. The occupancy grid values are encoded in the Data
field.
mapMsg
mapMsg = struct with fields:
MessageType: 'nav_msgs/OccupancyGrid'
Header: [1x1 struct]
Info: [1x1 struct]
Data: [251001x1 int8]
Use the rosReadOccupancyGrid
function to convert the ROS message to an occupancyMap
(Navigation Toolbox) object. Use the show
function to display the occupancy grid.
occupancyMapObj = rosReadOccupancyGrid(mapMsg); show(occupancyMapObj);
You can use the occupancyMap
(Navigation Toolbox) object functions to manipulate the occupancy grid. Use the inflate
function to expand the occupied regions. Write the occupancyMap object to a new ROS message using the rosWriteOccupancyGrid
function. Use the show
function to display the new occupancy grid.
inflate(occupancyMapObj,5) occupancyMapInflatedMsg = rosWriteOccupancyGrid(mapMsg,occupancyMapObj); show(occupancyMapObj);
Alternatively, you can create a binaryOccupancyMap
(Navigation Toolbox) object from a ROS occupancy grid message using the rosReadBinaryOccupancyGrid
function. A binary occupancy map holds discrete occupancy values of 0
or 1
at each cell whereas an occupancy map holds probability occupancy values that range between 0
and 1
at each cell. Use the show
function to display the binary occupancy grid.
binaryMapObj = rosReadBinaryOccupancyGrid(mapMsg); show(binaryMapObj);
Similarly, you can use the binaryOccupancyMap
(Navigation Toolbox) object functions to manipulate the binary occupancy grid. Use the inflate
function to alter the binary occupancy grid and create a new ROS message using the rosWriteBinaryOccupancyGrid
function. Use the show
function to display the new binary occupancy grid.
inflate(binaryMapObj,5) binaryMapInflatedMsg = rosWriteBinaryOccupancyGrid(mapMsg,binaryMapObj); show(binaryMapObj);