MATLAB Examples

Transform Laser Scan Data From A ROS Network

Transform laser scan data using a ROS transformation tree. When working with laser scan data, your sensor might not be mounted in the center of the robot. Many algorithms make this assumption, so that you might need to transform your data so it is relative to the robot center. This example uses a ROS transformation tree to receive the transformations between different coordinate frames. To transform the sensor data, you must be connected to a ROS network and have transformations available.

Setup and connect to a ROS network. Specify the IP address of the ROS device.

rosinit('192.168.203.129')
Initializing global node /matlab_global_node_25923 with NodeURI http://192.168.203.1:49997/

Create the ROS transformation tree using rostf. Get the transform between the '/camera_link' and '/base_link' coordinate frames. These coordinate frame names are dependent on your robot configuration.

tftree = rostf;
pause(1);
tf = getTransform(tftree,'/camera_link','/base_link',rostime('now'));

Extract the rotation and translation matrixes from the transform.

quat = [tf.Transform.Rotation.W,...
        tf.Transform.Rotation.X,...
        tf.Transform.Rotation.Y,...
        tf.Transform.Rotation.Z];
rotm = quat2rotm(quat);
trvec = [tf.Transform.Translation.X,...
         tf.Transform.Translation.Y ...
         tf.Transform.Translation.Z];

Create a homogeneous transform by combining the translation and rotation matrices.

tform = trvec2tform(trvec);
tform(1:3,1:3) = rotm(1:3,1:3);

Set up a subscriber to get laser scan data. Get the laser scan data as Cartesian points. Pad the points with zeros for the z-axis and convert them to homogeneous coordinates.

scansub = rossubscriber('/scan');
scan = receive(scansub)
cartScanData = scan.readCartesian;
cartScanData(:,3) = 0;
homScanData = cart2hom(cartScanData);
scan = 

  ROS LaserScan message with properties:

       MessageType: 'sensor_msgs/LaserScan'
            Header: [1×1 Header]
          AngleMin: -0.5216
          AngleMax: 0.5243
    AngleIncrement: 0.0016
     TimeIncrement: 0
          ScanTime: 0.0330
          RangeMin: 0.4500
          RangeMax: 10
            Ranges: [640×1 single]
       Intensities: [0×1 single]

  Use showdetails to show the contents of the message

Apply the homogeneous transform and convert scan data back to Cartesian points.

trPts = tform*homScanData';
cartScanDataTransformed = hom2cart(trPts');

Get the polar angles and ranges from the Cartesian Points.

[angles,ranges] = cart2pol(cartScanDataTransformed(:,1),...
                           cartScanDataTransformed(:,2));

Shutdown ROS network.

rosshutdown
Shutting down global node /matlab_global_node_25923 with NodeURI http://192.168.203.1:49997/