Main Content

Sign-Following Robot with ROS 2 in Simulink

This example shows how to control a simulated robot running on a separate ROS-based simulator over ROS 2 network using Simulink® software. You can run this example using a Docker container or a virtual machine.

In this example, you run a model that implements a sign-following algorithm and controls the simulated robot to follow a path based on signs in the environment. The algorithm receives the location information and camera information from the simulated robot, which is running in a separate ROS-based simulator. The algorithm detects the color of the sign and sends the velocity commands to turn the robot based on the color. In this example, the algorithm turns the robot to the left when it encounters a blue sign and to the right when it encounters a green sign. The robot stops when it encounters a red sign.

For an example that shows how to control a sign-following robot using MATLAB®, see Sign Following Robot with ROS in MATLAB.

Build ROS 2 Docker Image

You can download the ROS 2 Dockerfile by cloning ROS Toolbox Docker Files. Then, build a Docker image that has ROS 2 and Gazebo installed by following these steps:

  1. Open a Terminal in Linux and navigate to the "ros-docker-support/ros_humble/Ubuntu" folder.

  2. Build the Dockerfile by executing this command: docker build -t humble_docker_image_focal ros_humble/Ubuntu command. In this command, humble_docker_image_focal is the docker image name.

This image is based on Ubuntu® Linux® and supports the sign-following robot examples in ROS Toolbox.

Start Robot Simulator in Docker

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

Create a docker container by running this command below in a Linux Terminal:

docker run -it -p 8087:8080 --name ros2_sign_follower_robot humble_docker_image_focal

You can use these options with docker run command.

  • -it --- Start an interactive container.

  • -p --- Publish the container ports to the host computer. In this case, port 8087 on the host maps to port 8080 on the docker container.

  • --name --- Name the container. In this example you name the container ros2_sign_follower_robot.

  • This image shows the command line output when you create a Docker container:CreatingROS2Container.png

To view the Gazebo Sign Follower World, open any web browser and connect the host port you use to configure your Docker image. For this example, connect to localhost:8087.

This figure shows the Gazebo Sign Follower World.

StartingSignFollowerROS2.png

In the MATLAB Command Window, set the ROS_DOMAIN_ID environment variable to 25 to match the robot simulator settings and run ros2 topic list to verify that the topics from the robot simulator are visible in MATLAB.

setenv("ROS_DOMAIN_ID","25")
ros2("topic","list")
/camera/camera_info
/camera/image_raw
/clock
/cmd_vel
/imu
/joint_states
/odom
/parameter_events
/rosout
/scan
/tf

Get IP Address of Docker Container

To get the IP address of the Docker container, execute this command in a Terminal window.

docker inspect ros2_sign_follower_robot | grep IPAddress

IPAddressOfContainer2.png

Start Robot Simulator in VM

Alternatively, you can run this example using a virtual machine (VM). You can download a VM image that already has ROS and Gazebo software installed. This virtual machine is based on Ubuntu® Linux® and supports the examples in ROS Toolbox. To access the VM, follow these steps:

  1. Download and install the ROS Virtual Machine.

  2. Launch the VM.

  3. Start the Ubuntu® VM desktop.

  4. In the Ubuntu desktop, click the Gazebo ROS2 Maze icon to start the Gazebo world for this example.

In the MATLAB Command Window, set the ROS_DOMAIN_ID environment variable to 25 to match the robot simulator settings and run ros2 topic list to verify that the topics from the robot simulator are visible in MATLAB.

setenv("ROS_DOMAIN_ID","25")
ros2("topic","list")

Open Model and Configure Simulink

Setup the Simulink ROS preferences to communicate with the robot simulator.

Open the example model.

open_system("signFollowingRobotROS2.slx");

To configure the network settings for ROS 2.

  1. From the Prepare section of the Simulation tab, select ROS Toolbox > ROS Network.

  2. In Configure ROS Network Addresses, set the ROS 2 Domain ID value to 25.

  3. Click OK to apply changes and close the dialog.

At each time step, the algorithm detects a sign from the camera feed, decides which way to turn and drives the robot. The Image Processing subsystem of the model detects the signs.

open_system("signFollowingRobotROS2/Image Processing");

The Sign Tracking Logic subsystem implements a Stateflow® chart that takes in the detected image size and coordinates from Image Processing and provides linear and angular velocity to drive the robot.

open_system("signFollowingRobotROS2/Sign Tracking Logic");

Run Model

Run the model and observe the behavior of the robot in the robot simulator.

  • The video viewers show the actual camera feed and the detected sign image.

  • In the simulator, the robot follows the sign and turns based on the color.

  • Simulation stops when the robot reaches the red sign at the end.

RobotRunningROS2Latest.png