Published on *CPS-VO* (https://cps-vo.org)

In this tutorial we will show how to convert sensor_msgs/LaserScan into an image that can be used in Simulink. Once we have an image, we can use the Image Processing and Computer Vision toolboxes on the laser scan. The model for this tutorial can be found from the Files section, or in the github repository catvehicle_simulink (https://github.com/sprinkjm/catvehicle_simulink) A video that demonstrates and walks through this tutorial can be found at https://youtu.be/B8GBl_xoOTY

git clone https://github.com/sprinkjm/catvehicle_simulink.git

The sensor_msgs/LaserScan message is an array of distances, where each element in the array increments a fixed angle. It is possible to use rudimentary algorithms to detect nearby points or adjacent points, and infer values from these. However, it is also possible to consider that the returned points constitute an image, and therefore image processing and other kinds of filtering could be applied to the problem in order to identify regions of interest and/or objects in the field, based on their position.

Open up laserscan2image.slx from the github repository, and examine the different components of the model.

The model is decomposed into the following main pieces:

- Subscribe to laser data: this acquires laser information from the catvehicle's front laser scanner
- Unpack the sensors_msgs/LaserScan message: this transforms the ROS message into Simulink data for the ranges at each angle
- Coordinate transform: takes the range/angle data and places it into the Cartesian plane
*within the frame of the laser scanner* - Make an image: with the coordinates of each point, create a grayscale image that represents the scanner information

Below are some interesting pieces for each of these components.

The laser scanner is a 180 degree scanning laser, which provides 180 points with ranges between 1m and 80m. These messages are defined by ROS, and so we need to do a little effort to get them into a Simulink format.

When unpacking the data, we see how Simulink manages information; with the exception of "ReceivedLength" each one of these data values from the Bus are directly from the message. The received distances gives us an array of ranges:

dists 80 80 24.2 25.3

The angles_and_dists function, however, takes the single array of Ranges (which for this scanner should be 180 points) and creates a second array, which is the set of angles for each of these points. For a brief example, it would look like:

angles dists -pi 80 -pi + pi/180 80 -pi + 2pi/180 24.2 -pi + 3pi/180 25.3

This allows each index of angles to correspond to the distance at that angle.

This function is very simple, it multiplies the distances times the cosine (for x) and sine (for y) of the angle. This produces the location of each point in the Cartesian plane, within the laser sensor's frame of reference.

xs = dists.*cos(angles); ys = dists.*sin(angles); points = [xs,ys];

This is, by far, the most complex of the MATLAB function blocks, but it produces a Simulink-compatible image which is a projection of the (x,y) points into a (X,Y) image. By design, we expand each meter into 10 pixels in the image. Thus, our (x,y) of (80,160) possible points in meters, is mapped into a (800,1600) range for (X,Y). We create an image that is, therefore, X by Y pixels large, with the idea that the vehicle is looking from halfway down the image, to the right.

The coordinate system of the laser is for -y to be "up" in the image, and positive x to be "right" in the image, with (0,0) being halfway down the image, all the way to the left. Since a point (0,0) in the (image would be x=0 and y=-80, we can convert from (x,y) into (X,Y) by (X,Y) = (0,80)-(x,y). This is given in the code for the function block as:

% add 80 to all the y points pts(:,2) = 80 - pts(:,2);

And since we want to make 10 pixels for each meter:

% since we are building an 800x1600 image of the 80m (x) by 160m (y), we % will multiply the data by 10 and take the floor in order to show % occupancy near that point pts = 10*pts;

And then, the only thing to do is to transform each point into the image, by changing the default color (black) into white.

% iterate over these points, and set up a primitive grid map for ii=1:len ii_x = pts(ii,1); % the x point is in the first column ii_y = pts(ii,2); % the y point is in the second comlumn if(ii_x > 800) disp('ru roh'); end index_x = min(max(10,floor(ii_x)),800-10); index_y = min(max(10,floor(ii_y)),1600-10); % make a square here, of size 10x10 pixels % we must swap the (x,y) to (y,x) for MATLAB images img(index_y-5:index_y+5,index_x-5:index_x+5) = ... img(index_y-5:index_y+5,index_x-5:index_x+5) | 255; end

Now, you can see the laser returns as an image which can be used for image processing. Start up your favorite worldfile, and compare what you're seeing in rviz with what the laser returns show you in the image. Here you can also see where the car is sitting in rviz, and match the points on the rviz plane with points in the Simulink "To Video Display" image.

Now you're ready to use some image processing on the laser images!