Build a Map Using Lidar SLAM with ROS in MATLAB

In this example, you implement a ROS node that uses 2-D lidar data from a simulated robot to build a map of the robot's environment using simultaneous localization and mapping (SLAM). The node uses this map to estimate the position of the robot. You then generate C++ code and deploy the node to a remote device.

To assist the ROS node performing SLAM, you deploy a separate ROS node that controls the robot, making it move through its simulated environment by following the wall. This example uses ROS and MATLAB® for simulation, and MATLAB Coder™ for code generation and deployment.

Connect to Robot Simulator

Start a ROS-based simulator for a differential-drive robot, and configure a MATLAB connection with the robot simulator.

To follow along with this example, download a virtual machine using the instructions in Get Started with Gazebo and Simulated TurtleBot, and then follow these steps.

• Start the Ubuntu® virtual machine.

• On the Ubuntu desktop, click the Gazebo Lidar SLAM ROS icon to start the Gazebo world built for this example.

• Specify the IP address and port number of the ROS master to MATLAB so that it can communicate with the robot simulator. For this example, the ROS master is at the address 192.168.47.129 on port 11311.

• Start the ROS 1 network using rosinit.

masterIP = "192.168.47.129";
rosinit(masterIP,11311)

The value of the ROS_IP environment variable, 172.21.8.68, will be used to set the advertised address for the ROS node.
Initializing global node /matlab_global_node_59949 with NodeURI http://172.21.8.68:52485/ and MasterURI http://192.168.47.129:11311.

To ensure the robot is in the starting position before simulation, reset the Gazebo scene using the /gazebo/reset_simulation service. Create a rossvcclient object for the /gazebo/reset_simulation service and use the call object function to call the service and reset the Gazebo simulation scene.

gazeboResetClient = rossvcclient("/gazebo/reset_simulation",DataFormat="struct");
call(gazeboResetClient);

Create and Deploy Navigation Node

To map the entire room, create a node dedicated to robot navigation. The exampleHelperRobotWallFollowingNode node controls the robot to follow the wall around the perimeter of the room. Configure and deploy the helper ROS node that reads the lidar data from the robot and sends geometry_msgs/Twist messages on the /cmd_vel topic.

Create a MATLAB Coder configuration object that uses "Robot Operating System (ROS)" hardware. Before remote deployment, set these configuration parameters for the Linux virtual machine. Note that, if you are deploying to a different remote machine, you must change these to the appropriate parameters for your device.

coderConfig = coder.config("exe");
coderConfig.Hardware = coder.hardware("Robot Operating System (ROS)");
coderConfig.Hardware.BuildAction = "Build and Run";
coderConfig.Hardware.RemoteDeviceAddress = '192.168.47.129';
coderConfig.Hardware.RemoteDeviceUsername = 'user';
coderConfig.Hardware.RemoteDevicePassword = 'password';
coderConfig.Hardware.DeployTo = "Remote Device";
codegen exampleHelperRobotWallFollowingNode -config coderConfig

Configure ROS Communication

Create publishers and subscribers to relay messages to and from the simulation over the ROS network. The subscriber to receives the lidar data from the robot. To control the navigation node, create three publishers to three topics:

• /start_navigation topic — makes the robot start moving.

• /stop_navigation topic — makes the robot stop moving.

• /close_navigation topic — shuts down the navigation node.

startRobotPublisher = rospublisher("/start_navigation","std_msgs/Empty",DataFormat="struct"); % Publisher to start the robot
stopRobotPublisher = rospublisher("/stop_navigation","std_msgs/Empty",DataFormat="struct");   % Publisher to stop the robot
closeRobotPublisher = rospublisher("/close_navigation","std_msgs/Empty",DataFormat="struct"); % Publisher to close the navigation node

lidarSubscriber = rossubscriber("/scan","sensor_msgs/LaserScan",DataFormat="struct");         % Subscriber to pull lidar scans from the robot

Create the SLAM Object

Create a lidarSLAM (Navigation Toolbox) object. You must use the addScan object function to add lidar scans to the object to incrementally build the SLAM map and estimate the robot trajectory.

For code generation, you must specify these properties of the lidarSLAM object: map resolution, maximum lidar range, and maximum number of scans. To avoid errors, set the maximum number of scans significantly higher than the number of scans you expect to add to the map over the lifetime of the node.

The lidarSLAM object maintains a poseGraph (Navigation Toolbox), which holds all of the lidar scans and the relationships between them that define the map. The LoopClosureThreshold property of the lidarSLAM object determines how closely a scan must match the poseGraph to trigger a loop closure. Specifying too low a threshold can cause incorrect loop closures, which apply erroneous transformations to the map. Specifying too high a threshold can prevent the object from correcting the map over time as the robot revisits areas it has already explored. You can tune the threshold for the environment the robot is navigating.

slamObj = lidarSLAM(20,8,1000);     % Object that performs SLAM (map resolution 20 cells per meter, 8 m max lidar range, max of 1000 scans)
slamObj.LoopClosureThreshold = 350; % Raise threshold to prevent smearing

Run Control Loop

Run a control loop to perform SLAM and direct the robot. This loop consists of these actions:

• Get the latest lidar scan from the robot.

• Add the lidar scan to the lidarSLAM object. This incrementally builds the map of the environment and estimates the robot trajectory.

• Retrieve the latest position of the robot from the lidarSLAM object, and calculate how far the robot is from its initial position.

• Check if the robot has moved at least 2 meters from its initial position.

• Once the robot has moved more than 2 meters from its initial position, the node checks every subsequent scan to see if the robot has returned to that position. Once the robot is within 0.5 meters of its initial position, the control loop ends.

At the end of each iteration, send a start message to the robot to ensure the robot is moving as long as the control loop is active.

rate = rosrate(10);
robotLeftFlag = false; % Track if the robot has left the start area
while true
    [lidarMsg,status,~] = receive(lidarSubscriber); % Get the current lidar scan from the robot

    if status % If the scan is good, do SLAM
        angles = double(lidarMsg.AngleMin:lidarMsg.AngleIncrement:lidarMsg.AngleMax);
        ranges = double(lidarMsg.Ranges);
        scan = lidarScan(ranges,angles);                                                      % Build the scan object
        removeInvalidData(scan,RangeLimits=[1/slamObj.MapResolution slamObj.MaxLidarRange]);  % Remove invalid data to avoid errors
        addScan(slamObj,scan);                                                                % Add the scan
        show(slamObj);

        [~,poses] = scansAndPoses(slamObj);              % Get the robot poses from the SLAM map
        currentDistance = sqrt(sum(poses(end,1:2).^2));  % Take the most recent pose (current position of the robot) and calculate how far it is from the start

        if ~robotLeftFlag && currentDistance >= 2  % If the robot has left the start area, begin checking if the robot has returned to the start area
            robotLeftFlag = true;
        end
        if robotLeftFlag && currentDistance <= 0.5 % If the robot has completed a lap, end the control loop
            send(stopRobotPublisher,rosmessage(stopRobotPublisher))
            break % End control loop
        end
    end

    send(startRobotPublisher,rosmessage(startRobotPublisher)) % Tell the navigation node to move the robot
    waitfor(rate);
end

Visualize Loop Closures

Every time the lidarSLAM object registers a scan, it checks how similar the new scan is to known scans. If the new scan is similar enough to a known scan, the object adds a loop closure, which is an edge between two nearby nodes for which you know the relative positions with high certainty, and transforms the map to more accurately reflect the environment.

Extract the PoseGraph from the lidarSLAM object, and display the loop closures.

poseGraph = slamObj.PoseGraph;
show(poseGraph,IDs="loopclosures");
title(sprintf("Loop Closures: %d",poseGraph.NumLoopClosureEdges))

Shut Down

Send a message through the closeRobotPublisher publisher to shut down the navigation node. Then, reset the Gazebo scene.

send(closeRobotPublisher,rosmessage(closeRobotPublisher))
call(gazeboResetClient);

Generate and Deploy SLAM Node

After you verify the code, you can generate a ROS node for the SLAM routine using MATLAB Coder. You can then deploy this node on the remote virtual machine running Gazebo. Using deployment, you can run ROS nodes directly on remote machines. Create a MATLAB Coder configuration object that uses "Robot Operating System (ROS)" hardware. Before remote deployment, set these configuration parameters for the Linux virtual machine. Note that, if you are deploying to a different remote machine, you must change these to the appropriate parameters for your device.

Note: By default, the "Build and Load" build action deploys the node to the device, but does not automatically run it. If you want the node to run immediately after code generation, use the "Build and Run" build action, instead.

cfg = coder.config("exe");
cfg.Hardware = coder.hardware("Robot Operating System (ROS)");
cfg.Hardware.BuildAction = "Build and Load";
cfg.Hardware.RemoteDeviceAddress = '192.168.47.129';
cfg.Hardware.RemoteDeviceUsername = 'user';
cfg.Hardware.RemoteDevicePassword = 'password';
cfg.Hardware.DeployTo = "Remote Device";
codegen exampleHelperSlamManagerNode -config cfg

Rerun Deployed Nodes Using rosdevice

To rerun the deployed ROS nodes from MATLAB, create a rosdevice object, specifying the deviceAddress, username, and password arguments using the values corresponding to your remote device. The object establishes an SSH connection between the ROS device and MATLAB. Check the available nodes on the connected remote device. Verify that the deployed ROS nodes, exampleHelperSlamManagerNode and exampleHelperRobotWallFollowingNode, exist on the remote device.

gazeboVMDevice = rosdevice('192.168.47.129','user','password');
gazeboVMDevice.AvailableNodes

ans = 1×16 cell
    {'Comm_Debug_Node'}    {'Robot_Controller_Node'}    {'Robot_Controller_Simple_Node'}    {'SLAM_Controller_Node'}    {'SLAM_Controller_Simple_Node'}    {'SLAM_Node'}    {'Test_Node'}    {'detect_object'}    {'exampleHelperRobotWallFollowingNode'}    {'exampleHelperSlamManagerNode'}    {'hovering_example'}    {'kortex_arm_driver'}    {'lee_position_controller_node'}    {'roll_pitch_yawrate_thrust_controller_node'}    {'waypoint_publisher'}    {'waypoint_publisher_file'}

Run the ROS nodes deployed on the remote device by using the runNode function. The robot follows the perimeter wall and stops when it has completed one full lap.

runNode(gazeboVMDevice,'exampleHelperRobotWallFollowingNode')
runNode(gazeboVMDevice,'exampleHelperSlamManagerNode')

Receive and plot the map published by the deployed node.

mapSubscriber = rossubscriber("/slam_map","nav_msgs/OccupancyGrid", DataFormat="struct");

while isNodeRunning(gazeboVMDevice, 'exampleHelperSlamManagerNode')
    [mapMsg,status,~] = receive(mapSubscriber, 10);
    if status
        map = rosReadOccupancyGrid(mapMsg);
        show(map, FastUpdate=1);
        xlim([-1 5])
        ylim([-1 5])
        drawnow
    else
        break
    end
end

Disconnect from the ROS network after the nodes have finished.

rosshutdown

Shutting down global node /matlab_global_node_90273 with NodeURI http://172.21.8.68:55842/ and MasterURI http://192.168.47.129:11311.