Sign Following Robot with ROS in MATLAB

This example uses:

This example shows how to control a simulated robot running on a ROS-based simulator over a ROS network. You generate a ROS node for the control algorithm and deploy it to the remote device running ROS. The example shown here uses ROS and MATLAB for simulation, and MATLAB Coder™ for code generation and deployment. For the other examples with ROS 2 or Simulink®, see:

In this example, you implement a sign-following algorithm and control the simulated robot to follow a path based on signs in the environment. The algorithm receives the location and camera information from the simulated robot. The algorithm detects the color of the sign and sends velocity commands to turn the robot based on the color. In this example, the robot turns left when it encounters a blue sign and right when it encounters a green sign. The robot stops when it encounters a red sign.

Connect to Robot Simulator

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

Download a virtual machine using instructions in Get Started with Gazebo and Simulated TurtleBot, then follow these steps.

Start the Ubuntu® virtual machine desktop.
Click the Gazebo Sign Follower ROS icon to start the Gazebo world for this example.
Specify the IP address and port number of the ROS master in Gazebo so that MATLAB can communicate with the robot simulator. For this example, the ROS master in Gazebo has IP 172.18.228.122 and port number 11311, and your host computer address is 192.168.31.1.
Start the ROS 1 network using rosinit.

masterIP = '172.18.228.122';
rosinit(masterIP,11311);

Initializing global node /matlab_global_node_12033 with NodeURI http://172.18.228.249:60144/ and MasterURI http://172.18.228.122:11311.

Configure ROS Communication

Create publishers and subscribers to relay messages to and from the robot simulator over ROS network. You need subscribers for the image and odometry data. To control the robot, set up a publisher to send velocity commands using /cmd_vel. The rossubscriber and rospublisher functions support code generation for message structures only. To return a message structure, specify the name-value pair argument, "DataFormat","struct", when creating subscribers and publishers.

imgSub = rossubscriber("/camera/rgb/image_raw","sensor_msgs/Image",DataFormat = "struct");
odomSub = rossubscriber("/odom","nav_msgs/Odometry",DataFormat = "struct");
[velPub, velMsg] = rospublisher("/cmd_vel", "geometry_msgs/Twist",DataFormat = "struct");

Create a rosbagwriter object to log the subscribed image and odometry data that the robot publishes.

bagWriter = rosbagwriter("sign_following_data.bag")

bagWriter = 
  rosbagwriter with properties:

       FilePath: 'Z:\32\psahu.Bdoc23a.j2115093\sign_following_data.bag'         
      StartTime: 0                                                              
        EndTime: 0                                                              
    NumMessages: 0                                                              
    Compression: 'uncompressed'                                                 
      ChunkSize: 786432                                                    Bytes
       FileSize: 4117                                                      Bytes

Define the image processing color threshold parameters. Each row defines the threshold values for the corresponding color.

colorThresholds = [100 255 0 55 0 50; ... % Red
                   0 50 50 255 0 50; ...  % Green
                   0 40 0 55 50 255]';    % Blue

Create Sign-following Controller Using Stateflow® Chart

This example provides a helper MATLAB Stateflow® chart that takes as inputs the image size, coordinates from a processed image, and robot odometry poses. The chart returns the linear and angular velocity to drive the robot.

controller = ExampleHelperSignFollowingControllerChart;
open("ExampleHelperSignFollowingControllerChart");

Run Control Loop

Run the controller to receive images and move the robot to follow the sign. The controller performs these steps:

Get the latest image and odometry message from the ROS network.
Run the algorithm for detecting image features (ExampleHelperSignFollowingProcessImg) function.
Generate control commands from the Stateflow® chart using step.
Publish the velocity control commands to the ROS network.

To visualize the masked image that the robot sees, change the value of doVisualization to true.

ExampleHelperSignFollowingSetupPreferences;
doVisualization = false;
r = rosrate(10);
receive(imgSub); 
receive(odomSub);
while(~controller.done)
    % Get the latest sensor messages and process them.
    imgMsg = imgSub.LatestMessage;
    odomMsg = odomSub.LatestMessage;
    [img,pose] = ExampleHelperSignFollowingROSProcessMsg(imgMsg, odomMsg);
    
    % Log the subscribed image and odometry data into a bag file.
    currentTime = rostime("now");
    write(bagWriter,imgSub.TopicName,currentTime,imgMsg);
    write(bagWriter,odomSub.TopicName,currentTime,odomMsg);

    % Run the vision and control functions.
    [mask,blobSize,blobX] = ExampleHelperSignFollowingProcessImg(img, colorThresholds);
    step(controller,"blobSize",blobSize,"blobX",blobX,"pose",pose);
    v = controller.v;
    w = controller.w;
    
    % Publish the velocity commands.
    velMsg.Linear.X = v;
    velMsg.Angular.Z = w;
    send(velPub,velMsg);
    
    % Optionally visualize
    % NOTE: Visualizing data will slow down the execution loop.
    % If you have Computer Vision Toolbox, we recommend using
    % vision.DeployableVideoPlayer instead of imshow.
    if doVisualization
        imshow(mask);
        title(['Linear Vel: ' num2str(v) ' Angular Vel: ' num2str(w)]);
        drawnow('limitrate');
    end
    % Pace the execution loop.
    waitfor(r);
end

The robot follows the signs and stops at the final STOP sign. This figure shows the robot in motion.

Delete the rosbagwriter object to close the bag file and reset the Gazebo scene after simulation using the /gazebo/reset_simulation service. Create a rossvcclient object for the service. Use the call object function to call the service and reset the Gazebo simulation scene.

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

Visualize Logged ROS Messages

Open the ROS Data Analyzer app by entering rosDataAnalyzer in the MATLAB® Command Window. You can also launch the app from the Apps section from the MATLAB toolstrip.

Click Open and load the generated sign_following_data.bag file. Inspect the Topic List and Source Details panel on the left side of the app to check the bag file information. Launch Image and Odometry visualizers from the toolstrip and select data sources. Use the Playback panel controls to play the bag file and visualize the logged messages.

Generate and Deploy ROS Node

After you verify the controller, you can generate a ROS node for the sign-following robot algorithm using MATLAB Coder™ software. You can then deploy this node on the remote virtual machine running Gazebo. Using deployment, you can run ROS nodes on remote machines directly, resulting in faster executions. Create a MATLAB Coder configuration object that uses "Robot Operating System (ROS)" hardware.

cfg = coder.config("exe");
cfg.Hardware = coder.hardware("Robot Operating System (ROS)");

Set configuration parameters. You can adjust these values so they are suitable for your remote device and verify them before deployment. The robot follows the signs and stops at the final STOP sign.

cfg.Hardware.DeployTo = "Remote Device";
cfg.Hardware.RemoteDeviceAddress = '192.168.192.129';
cfg.Hardware.RemoteDeviceUsername = "user";
cfg.Hardware.RemoteDevicePassword = "password";

Set the build action to "Build and Run" to ensure that the deployed ROS node starts running after code generation.

cfg.Hardware.BuildAction = "Build and run";

Generate the ROS node and deploy the controller. The DeploySignFollowingRobotROS function contains the controller algorithm code.

codegen DeploySignFollowingRobotROS -config cfg

Reset the Gazebo scene after the node executes by calling the rossvcclient object, gazeboResetClient.

call(gazeboResetClient);

Rerun Deployed Node Using `rosdevice`

To rerun the deployed ROS node, create a rosdevice object. Specify the deviceAddress, username, and password inputs to establish an SSH connection between the ROS device and MATLAB.

gazeboVMDevice = rosdevice('192.168.192.129','user','password');

Check the available nodes on the connected remote device. Verify that the deployed ROS node, deploysignfollowingrobotros, exists on the remote device.

gazeboVMDevice.AvailableNodes

Run the ROS node on the remote device using runNode function. The robot follows the signs and stops at the final STOP sign.

runNode(gazeboVMDevice,'DeploySignFollowingRobotROS')