work with specialized ros messages -凯发k8网页登录

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 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: [1×1 struct]
         height: 0
          width: 0
       encoding: ''
    isbigendian: 0
           step: 0
           data: [0×1 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: [1×1 struct]
         height: 480
          width: 640
       encoding: 'rgb8'
    isbigendian: 0
           step: 1920
           data: [921600×1 uint8]

the data property stores raw image data that cannot be used directly for processing and visualization in matlab. you can use the 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, returns the image in a standard 480-by-640-by-3 uint8 format. view this image using the function.

imshow(imageformatted)

matlab® supports all ros image encoding formats, and handles the complexity of converting the image data. in addition to color images, matlab also supports monochromatic and depth images.

additionally, matlab provides the 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 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 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 . 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: [1×1 struct]
         format: ''
           data: [0×1 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: [1×1 struct]
         format: 'bgr8; jpeg compressed bgr8'
           data: [30376×1 uint8]

similar to the image message, you can use to obtain the image in standard rgb format. even though the original encoding for this compressed image is bgr8, does the conversion.

compressedformatted = rosreadimage(imgcomp);

visualize the image using the 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 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 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: [1×1 struct]
         height: 0
          width: 0
         fields: [0×1 struct]
    isbigendian: 0
      pointstep: 0
        rowstep: 0
           data: [0×1 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: [1×1 struct]
         height: 480
          width: 640
         fields: [4×1 struct]
    isbigendian: 0
      pointstep: 32
        rowstep: 20480
           data: [9830400×1 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 function.

xyz = rosreadxyz(ptcloud)

xyz = 307200×3 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 = 193359×3 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 .

rgb = rosreadrgb(ptcloud)

rgb = 307200×3
    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. 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 function. the loaded point cloud message contains four fields x, y, z, and rgb.

fieldnames = rosreadallfieldnames(ptcloud)

fieldnames = 1×4 cell
    {'x'}    {'y'}    {'z'}    {'rgb'}

you can access the corresponding data for any field using the function. 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 to uint8 and reshaping the result. use the result from the 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

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 to create the message.

emptyoctomap = rosmessage("octomap_msgs/octomap",dataformat="struct")

emptyoctomap = struct with fields:
    messagetype: 'octomap_msgs/octomap'
         header: [1×1 struct]
         binary: 0
             id: ''
     resolution: 0
           data: [0×1 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: [1×1 struct]
         binary: 1
             id: 'octree'
     resolution: 0.0250
           data: [3926×1 int8]

create an (navigation toolbox) object from the ros message using the 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 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 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 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: [1×1 struct]
             height: 0
              width: 0
    distortionmodel: ''
                  d: [0×1 double]
                  k: [9×1 double]
                  r: [9×1 double]
                  p: [12×1 double]
           binningx: 0
           binningy: 0
                roi: [1×1 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 (computer vision toolbox) function can be used to create (computer vision toolbox) and (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 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: [1×1 struct]
          anglemin: 0
          anglemax: 0
    angleincrement: 0
     timeincrement: 0
          scantime: 0
          rangemin: 0
          rangemax: 0
            ranges: [0×1 single]
       intensities: [0×1 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: [1×1 struct]
          anglemin: -0.5467
          anglemax: 0.5467
    angleincrement: 0.0017
     timeincrement: 0
          scantime: 0.0330
          rangemin: 0.4500
          rangemax: 10
            ranges: [640×1 single]
       intensities: [0×1 single]

you can get the scan angles from the ros message using the 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 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 function:

rosplot(scan,"maximumrange",5)

create a lidarscan object from a ros message using the 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: [640×1 double]
       angles: [640×1 double]
    cartesian: [640×2 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 laser scan messages. use 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: [1×1 struct]
           info: [1×1 struct]
           data: [0×1 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: [1×1 struct]
           info: [1×1 struct]
           data: [251001×1 int8]

use the function to convert the ros message to an (navigation toolbox) object. use the show function to display the occupancy grid.

occupancymapobj = rosreadoccupancygrid(mapmsg);
show(occupancymapobj);

you can use the (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 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 (navigation toolbox) object from a ros occupancy grid message using the 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 (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 function. use the show function to display the new binary occupancy grid.

inflate(binarymapobj,5)
binarymapinflatedmsg = roswritebinaryoccupancygrid(mapmsg,binarymapobj);
show(binarymapobj);