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 typesensor_msgs/Image.

Create an empty image message usingrosmessageto 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 theimgvariable is loaded fromspecialROSMessageData.mat.

Inspect the image message variableimgin your workspace. The size of the image is stored in theWidthandHeightproperties. ROS sends the actual image data using a vector in theDataproperty.

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]

TheDataproperty stores raw image data that cannot be used directly for processing and visualization in MATLAB. You can use therosReadImagefunction to retrieve the image in a format that is compatible with MATLAB.

imageFormatted = rosReadImage(img);

The original image has a rgb8 encoding. By default,rosReadImagereturns the image in a standard 480-by-640-by-3uint8format. View this image using theimshowfunction.

imshow(imageFormatted)

MATLAB® supports all ROS image encoding formats, androsReadImagehandles the complexity of converting the image data. In addition to color images, MATLAB also supports monochromatic and depth images.

Additionally, MATLAB provides therosWriteImagefunction 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 therosWriteImagefunction. Since the modified image only has 1 channel and is of typeuint8, use themono8encoding.

imageMsg = rosWriteImage(emptyimg,thresholdImg,Encoding="mono8");

Compressed 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 usingrosmessage. Compressed images in ROS have the message typesensor_msgs/CompressedImageand 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 fromspecialROSMessageData.mat.

Inspect theimgcompvariable that was captured by a camera. TheFormatproperty captures all the information that MATLAB needs to decompress the image data stored inData.

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 userosReadImageto obtain the image in standard RGB format. Even though the original encoding for this compressed image isbgr8,rosReadImagedoes the conversion.

compressedFormatted = rosReadImage(imgcomp);

Visualize the image using theimshowfunction.

imshow(compressedFormatted)

Most image formats are supported for the compressed image message type. The16UC1and32FC1encodings are not supported for compressed depth images. Monochromatic and color image encodings are supported.

Point Clouds

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 issensor_msgs/PointCloud2and 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 theptcloudvariable 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 theDataproperty of the message. You can extract thex,y,zcoordinates as anN-by-3 matrix by calling therosReadXYZfunction.

xyz = rosReadXYZ(ptcloud)

xyz =307200x3 single matrixNaN 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 ⋮

NaNin the point cloud data indicates that some of thex,y,zvalues are not valid. This is an artifact of the Kinect® sensor, and you can safely remove allNaNvalues.

xyzValid = xyz(~isnan(xyz(:,1)),:)

xyzValid =193359x3 single matrix0.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 torosReadRGB.

rgb = rosReadRGB(ptcloud)

rgb =307200×30.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 therosPlotfunction.rosPlotautomatically extracts thex,y,zcoordinates and the RGB color values (if they exist) and show them in a 3-D scatter plot. TherosPlotfunction ignores allNaNx,y,zcoordinates, even if RGB values exist for that point.

rosPlot(ptcloud)

Examine all the stored fields in the point cloud message using therosReadAllFieldNamesfunction. The loaded point cloud message contains four fieldsx,y,z, andrgb.

fieldNames = rosReadAllFieldNames(ptcloud)

fieldNames =1x4 cell{'x'} {'y'} {'z'} {'rgb'}

You can access the corresponding data for any field using therosReadFieldfunction. You must to unpack the returned data manually, depending on how it is formatted. For example, the RGB image can be extracted by type casting the data touint8and reshaping the result. Use the result from therosReadAllFieldNamesfunction for input validation.

如果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

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.

Userosmessageto 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, anoctomapmessage that is fully populated and is stored in theoctomapvariable loaded fromspecialROSMessageData.mat.

Inspect the variableoctomapin your workspace. TheDatafield 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 anoccupancyMap3D(Navigation Toolbox)object from the ROS message using therosReadOccupancyMap3Dfunction. Display the 3D occupancy map using theshowfunction.

occupancyMap3DObj = rosReadOccupancyMap3D(octomap); show(occupancyMap3DObj)

Quaternion Messages

Quaternions are commonly used in robotics to express orientation. Userosmessageto 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 fromspecialROSMessageData.mat. Inspect the variablequatMsgin your workspace.

quatmsg

quatmsg =struct with fields:MessageType: 'geometry_msgs/Quaternion' X: 0 Y: 0 Z: 0.7071 W: 0.7071

Create aquaternionobject from a ROS message using therosReadQuaternionfunction. Thequaternionobject 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 usingrotatepointfunction. Visualize the two points

cartesianPoint = [1,0,1]; plot3(cartesianPoint(1),cartesianPoint(2),cartesianPoint(3),"bo") holdonplot3 ([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") gridon

Camera Info Messages

Camera calibration is a commonly used procedure in robotics vision applications. ROS providessensor_msgs/CameraInfomessage type to publish calibration information. Userosmessageto 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]

值得注意的是,消息存储后来cesKandPas vectors. ROS requires these matrices to be stored in row-major format. MATLAB stores matrices in column-major, hence extracting theKandPmatrices requires reshaping and transposing.

TheestimateCameraParameters(Computer Vision Toolbox)function can be used to createcameraParameters(Computer Vision Toolbox)andstereoParameters(Computer Vision Toolbox)objects. You can createsensor_msgs/CameraInfomessages from these objects using therosWriteCameraInfofunction. The objects must be converted to structures before use. Load the camera calibration structures.

load("calibrationStructs.mat")

For convenience, the variableparamsloaded fromcalibrationStructs.matis a fully populatedcameraParametersstruct. Write thecameraParametersstruct to a new ROS message using therosWriteCameraInfofunction.

msg = rosWriteCameraInfo(emptycamerainfomsg,params);

The following table shows the correspondence between the cameraParameters object and the ROS message.

exampleHelperShowCameraParametersTable

ans=5×2 tableROS 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 ofparams.

K = reshape(msg.K,3,3)'

K =3×3714.1885 0 563.6481 0 710.3785 355.7254 0 0 1.0000

intrinsicMatrix = params.IntrinsicMatrix'

intrinsicMatrix =3×3714.1885 0 563.6481 0 710.3785 355.7254 0 0 1.0000

For convenience, the variablestereoParamsloaded fromcalibrationStructs.matis a fully populatedstereoParametersstruct. Write thestereoParametersstruct to two new ROS messages using therosWriteCameraInfofunction.

[msg1,msg2] = rosWriteCameraInfo(msg,stereoParams);

The following table shows the correspondence between the stereoParameters object and the ROS message.

exampleHelperShowStereoParametersTable

ans=2×2 tableROS 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 andstereoParamsmatch.

R1 = reshape(msg1.R,3,3)'; R2 = reshape(msg2.R,3,3)'; R = R1\R2

R =3×31.0000 -0.0002 -0.0050 0.0002 1.0000 -0.0037 0.0050 0.0037 1.0000

rotationOfCamera2 = stereoParams.RotationOfCamera2

rotationOfCamera2 =3×31.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 andstereoParamsmatch.

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 providessensor_msgs/LaserScanmessage type to publish laser scan messages. Userosmessageto create a laser scan message and observe the fields.

emptyscan = rosmessage("sensor_msgs/LaserScan","DataFormat","struct")

emptyscan =struct with fields:MessageType:“sensor_msgs /提升”标题:[1 x1struct] 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,emptyscandoes not contain any meaningful data. For convenience, a laser scan message that is fully populated and is stored in thescanvariable was loaded fromspecialROSMessageData.mat.

Inspect thescanvariable. The primary data in the message is in theRangesproperty. The data inRangesis a vector of obstacle distances recorded at small angle increments.

scan

scan =struct with fields:MessageType:“sensor_msgs /提升”标题:[1 x1struct] 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 therosReadScanAnglesfunction. Visualize the scan data in polar coordinates using thepolarPlotfunction.

angles = rosReadScanAngles(scan); figure polarplot(angles,scan.Ranges,LineWidth=2) title("Laser Scan")

You can get the measured points in Cartesian coordinates using therosReadCartesianfunction.

xy = rosReadCartesian(scan);

This populatesxywith a list of[x,y]coordinates that were calculated based on all valid range readings. Visualize the scan message using therosPlotfunction:

rosPlot(scan,"MaximumRange",5)

Create alidarScanobject from a ROS message using therosReadLidarScanfunction. ThelidarScanobject contains ranges, angles, and Cartesian points and provides additional functionalities, such as transforming the scanned points. Use thetransformScanfunction to rotate the scan point and visualize it usingplot.

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 providesnav_msgs/OccupancyGridmessage type to publish laser scan messages. Userosmessageto 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 thatemptyMapdoes not contain any meaningful data. For convenience, an occupancy grid message that is fully populated and is stored in themapMsgvariable loaded fromspecialROSMessageData.mat.

Inspect themapMsgvariables. The occupancy grid values are encoded in theDatafield.

mapMsg

mapMsg =struct with fields:MessageType: 'nav_msgs/OccupancyGrid' Header: [1x1 struct] Info: [1x1 struct] Data: [251001x1 int8]

Use therosReadOccupancyGridfunction to convert the ROS message to anoccupancyMap(Navigation Toolbox)object. Use theshowfunction to display the occupancy grid.

occupancyMapObj = rosReadOccupancyGrid(mapMsg); show(occupancyMapObj);

You can use theoccupancyMap(Navigation Toolbox)object functions to manipulate the occupancy grid. Use theinflatefunction to expand the occupied regions. Write the occupancyMap object to a new ROS message using therosWriteOccupancyGridfunction. Use theshowfunction to display the new occupancy grid.

inflate(occupancyMapObj,5) occupancyMapInflatedMsg = rosWriteOccupancyGrid(mapMsg,occupancyMapObj); show(occupancyMapObj);

Alternatively, you can create abinaryOccupancyMap(Navigation Toolbox)object from a ROS occupancy grid message using therosReadBinaryOccupancyGridfunction. A binary occupancy map holds discrete occupancy values of0or1at each cell whereas an occupancy map holds probability occupancy values that range between0and1at each cell. Use theshow函数显示二进制占用网格。

binaryMapObj = rosReadBinaryOccupancyGrid(mapMsg); show(binaryMapObj);

Similarly, you can use thebinaryOccupancyMap(Navigation Toolbox)object functions to manipulate the binary occupancy grid. Use theinflatefunction to alter the binary occupancy grid and create a new ROS message using therosWriteBinaryOccupancyGridfunction. Use theshowfunction to display the new binary occupancy grid.

inflate(binaryMapObj,5) binaryMapInflatedMsg = rosWriteBinaryOccupancyGrid(mapMsg,binaryMapObj); show(binaryMapObj);