%% 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. Specify the IP address of the ROS device. For this example, a sample network is already set up with an existing transformation tree.

rosinit('192.168.233.131')

%
% 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)
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

%
% Copyright 2019 The MathWorks, Inc.
%