Main Content

Lane Detection in ROS 2 Using Deep Learning with MATLAB

This example demonstrates how to use deep convolutional neural network to detect lanes from images published on a topic in the ROS 2 network. It then shows how to generate CUDA® optimized code and executable for the lane detection algorithm and deploy it as a ROS 2 node.

In this example, you first read traffic video as the input and publish the frames as sensor_msgs/Image messages to a topic on the ROS 2 network. Then you detect the left and right lane boundaries corresponding to the ego vehicle in every frame, annotate the input image with the detections, and publish them to a topic in the ROS 2 network. Finally, you generate CUDA® optimized code using ROS 2 for the lane detection.

This example uses the pretrained lane detection network from the Lane Detection Optimized with GPU Coder example of GPU Coder Toolbox™. For more information, see Lane Detection Optimized with GPU Coder (GPU Coder) (GPU Coder).

Third-Party Prerequisites

Overview

This example uses the video caltech_cordova1.avi for the pretrained neural network for the lanes detection. The pretrained lane detection network can detect and output lane marker boundaries from an image and is based on the AlexNet network. The last few layers of the AlexNet network are replaced by a smaller fully connected layer and regression output layer. For more information on the lane detection network, please refer to the Lane Detection Optimized with GPU Coder (GPU Coder) example.

This example uses two subscribers and publishers each:

  • rawImagePublisher - Publishes raw images to the topic /image_topic

  • rawImageSubscriber - Subscribes raw images from the topic /image_topic

  • processedImagePublisher - Publishes processed images after the lane detection to the topic /image_processed_topic

  • processedImageSubscriber - Subscribes processed images from the topic /image_processed_topic

The lane detection algorithm follows these steps:

  1. The traffic video as image frames are multiplied by a constant factor of 255 and published by rawImagePublisher as ROS 2 images. The ROS 2 image is received and read by the subscriber rawImageSubscriber and then fed to the pretrained network from Deep Learning Toolbox™ after being resized to 227-by-227-by-3. If the left and right lane boundaries are detected, the parabolic coefficients to model the trajectories of the lane boundaries are obtained.

  2. The processed images are then written in ROS 2 images and published by processedImagePublisher.

  3. The subscriber processedImageSubscriber receives the processed ROS2 images.

  4. The processed images are displayed.

Download Video and PreTrained Network

Download video.

if ~exist('./caltech_cordova1.avi','file')
    url = 'https://www.mathworks.com/supportfiles/gpucoder/media/caltech_cordova1.avi';
    websave('caltech_cordova1.avi',url);
end

The helperDownloadPreTrainedLaneDetector function downloads the trainedLaneNet.mat if it is not already present.

helperDownloadPreTrainedLaneDetector

The helperRawImageNode function reads the video and publishes the frames as ROS 2 images.

type helperRawImageNode.m

The helperImageProcessingNode function reads the raw images from the ROS 2 network, detects and marks the lanes on the raw images, and publishes the processed images.

type helperImageProcessingNode.m

Code Generation

Configure the MATLAB® code generation configuration object settings to generate the CUDA optimized code as a ROS 2 node. Generate code for helperRawImageNode and helperImageProcessingNode functions. MATLAB Coder generates a separate ROS 2 node for each function. For helperImageProcessingNode function, create a DeepLearningConfig object and select the appropriate DNN library and enable GPU configuration.

cfg = coder.config("exe");
cfg.Hardware = coder.hardware("Robot Operating System 2 (ROS 2)");
cfg.Hardware.DeployTo = "Localhost";
cfg.Hardware.BuildAction = "Build and load";
cfg.HardwareImplementation.ProdHWDeviceType = "Intel->x86-64 (Windows64)";
cfg.DeepLearningConfig = coder.DeepLearningConfig('cudnn');
cfg.GpuConfig = coder.GpuCodeConfig;
cfg.GpuConfig.Enabled = true;
codegen -config cfg helperRawImageNode
codegen -config cfg helperImageProcessingNode

Run Generated ROS 2 Nodes and Visualize Processed Images

Use the ros2device object to connect to the ROS 2 device where the code is deployed. For this example, connect to the localhost, and list the available nodes.

d = ros2device('localhost');
d.AvailableNodes

To receive and display processed images, create a ROS 2 node with a subscriber which receives the processed images.

node3 = ros2node("/imageDisplayNode");
processedImaegSubscriber = ros2subscriber(node3,"/image_processed_topic","sensor_msgs/Image", "History","keepall"); 

To run the generated image processing algorithm node, use the runNode object function of the ros2device object.

runNode(d,'helperImageProcessingNode');
% Pause for some time to wait for the node running.
pause(5)
isNodeRunning(d,'helperImageProcessingNode')

Set up visualization.

fig = figure;
set(fig, 'Visible', 'on');
fig.Name ='Lane Detection with GPU Coder';

Run the generated raw image publisher node. Visualize the processed images.

runNode(d,'helperRawImageNode');
% Terminate this loop by 'ctrl' + 'C' if the video has reached to the end.
emptyCount = 0;
for i = 1:150
    [imageMsg,status,statustext] = receive(processedImaegSubscriber,10);
    if status == 1 
        if ~isempty(imageMsg.data)
           imageMsg.encoding = 'rgb8';
           imageData = rosReadImage(imageMsg,0,"Encoding","rgb8");
           imshow(imageData);
           drawnow;
           % Pause for a short time for displaying the image.
           pause(0.01)
        end
    else
        if emptyCount <= 3
            % Count on one if no image message received.
            emptyCount = emptyCount + 1;
        else
            emptyCount > 3
            break
        end
    end
end

Note the yellow markings for the left and right lane boundaries in the processed images as shown below:

untitled1.png

To stop the running nodes, use the stopNode object function of the ros2device object.

stopNode(d,'helperRawImageNode')
stopNode(d,'helperImageProcessingNode')