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 localization algorithms make the assumption that your sensor is mounted in the center of the robot. So depending on your robot configuration, you must transform your laser scan data so it is relative to the robots 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. For this example, a sample network is already set up with an existing transformation tree. You must specify the IP address of your ROS device, on which the transformations are published.

rosinit('192.168.233.131')

Initializing global node /matlab_global_node_68056 with NodeURI http://192.168.233.1:62899/

Create the ROS transformation tree using rostf. The function connects to the ROS parameter server for the network. 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 matrices 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)

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

cartScanData = scan.readCartesian;
cartScanData(:,3) = 0;
homScanData = cart2hom(cartScanData);

Ensure that there is something within scanning distance of your robot. If nothing is detected, a laser scan will contain only NaN values, resulting in an error from cart2hom.

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_68056 with NodeURI http://192.168.233.1:62899/