In this tutorial, we will learn how to create an autonomous mobile robot from scratch using Gazebo. We will learn how to create an environment for the robot to move around in. We will also learn how to integrate ROS 2 and Gazebo so that we can control the robot by sending it velocity commands. Here is what you will build:
The type of robot we will create is an autonomous differential drive mobile warehouse robot. We will build the entire SDF file (Simulation Description Format) from scratch. Our simulated robot will be similar to the one below created by Fetch Robotics, a mobile robotics company based in Silicon Valley in California.

Real-World Applications
Mobile warehouse robots are used extensively in the industry. Amazon uses these robots to transport shelves around their fulfillment centers to make sure customers receive their goods as quickly as possible.
Roboticists like to simulate robots before building them in order to test out different algorithms. You can imagine the cost of making mistakes with a physical robot can be high (e.g. crashing a mobile robot into a wall at high speed means lost money).
Prerequisites
- You have ROS 2 installed in Ubuntu Linux (I’m using ROS 2 Foxy)
- You know how to create a basic mobile robot in Gazebo.
Build the Warehouse Robot
Create Model.config
Create a folder for the model.
mkdir -p ~/.gazebo/models/mobile_warehouse_robot
Create a model config file. This file will contain a description of the model.
gedit ~/.gazebo/models/mobile_warehouse_robot/model.config
Modify this model.config file. You can see this file contains fields for the name of the robot, the version, the author (that’s you), your e-mail address, and a description of the robot.

Save the file, and then close it.
Download the Mesh Files
Mesh files help make your robots look more realistic than just using basic shapes.
Download the warehouse robot mesh file. The warehouse robot mesh file is at this link. To download it, you need to open a new terminal window, and type the following commands:
cd ~/.gazebo/models
wget -q -R *index.html*,*.tar.gz --no-parent -r -x -nH http://models.gazebosim.org/warehouse_robot/
You can see on the website that the name of the actual robot is robot.dae. You will see this later in our SDF file.

Now download the Hokuyo Laser Range Finder mesh file.
cd ~/.gazebo/models
wget -q -R *index.html*,*.tar.gz --no-parent -r -x -nH http://models.gazebosim.org/hokuyo/
You can see that the dae file we need for the mesh is hokuyo.dae. You will see this filename inside the sdf file we’ll create in the next section.

Create Model.sdf
Now, let’s create an SDF (Simulation Description Format) file. This file will contain the tags that are needed to create an instance of the mobile_warehouse_robot model. Our robot will have three wheels: two wheels in the front and a caster wheel in the back. On top of the robot, we will mount the laser range finder that will send out beams in a 180-degree radius in order to detect obstacles.
gedit ~/.gazebo/models/mobile_warehouse_robot/model.sdf
Here is my sdf file. You can copy and paste those lines inside your sdf file.
Save the file and close it to return to the terminal.
Test Your Robot
Now let’s run Gazebo so that we can see our model. Type the following command:
gazebo
On the left-hand side, click the “Insert” tab.
On the left panel, click “Mobile Warehouse Robot”. You should see a warehouse robot. You can place it wherever you want by clicking inside the environment.
Click on your model to select it.

Go back to the terminal window, and type CTRL + C to close Gazebo.
Integrate ROS 2 and Gazebo
Install gazebo_ros_pkgs
Open a new terminal window, and install the packages that will enable you to use ROS 2 to interface with Gazebo. We need to install a whole bunch of stuff, including the differential drive plugin that will enable us to control the speed of our robot using ROS 2 commands.
sudo apt install ros-foxy-gazebo-ros-pkgs
Test Your ROS 2 and Gazebo Integration
Open a new terminal window.
Install some more tools.
sudo apt install ros-foxy-ros-core ros-foxy-geometry2
Open a new terminal window.
Load a demo robot. This is all one command.
gazebo --verbose /opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_diff_drive_demo.world

Let’s see what commands are available to us. Open up the sdf file.
gedit /opt/ros/foxy/share/gazebo_plugins/worlds/gazebo_ros_diff_drive_demo.world

Open up a new terminal window, and type the following command to make the robot move forward at a speed of 1.0 meters per second:
ros2 topic pub /demo/cmd_demo geometry_msgs/Twist '{linear: {x: 1.0}}' -1
The robot will begin moving forward.
Close all the active programs by pressing CTRL + C in all terminals.
Now launch Gazebo again.
gazebo
Insert your “Mobile Warehouse Robot” model into the environment.
In the terminal window, you should see the following output from ROS 2.

Let’s see the active topics.
ros2 topic list -t

The /demo/cmd_vel topic is where you can give your robot velocity commands. Let’s make the robot drive forward at 0.05 meters per second.
ros2 topic pub /demo/cmd_vel geometry_msgs/Twist '{linear: {x: 0.05}}' -1
You will see the robot bob up and down for a bit and then stabilize as it moves forward. The bobbing action is due to the uneven weight caused by the beacon and the laser range finder on top of the robot.
Feel free to tweak the parameters (radii, positioning of the wheels, etc.) to see if you get smoother performance.
Build a Warehouse
Now we are going to build a warehouse for our robot to move around in.
Create Model.config
Create a folder for the model.
mkdir -p ~/.gazebo/models/small_warehouse
Create a model config file. This file will contain a description of the model.
gedit ~/.gazebo/models/small_warehouse/model.config
Add these lines to the file, and then Save. You can see this file contains fields for the name of the robot, the version, the author (that’s you), your email address, and a description of the robot.
Save the file, and then close it.
Create Model.sdf
Now, let’s create an SDF (Simulation Description Format) file. This file will contain the tags that are needed to create an instance of the small_warehouse model.
gedit ~/.gazebo/models/small_warehouse/model.sdf
Write these lines inside the sdf file. You can see how many lines of code there are. It takes a lot of code to create even the simplest of models in Gazebo.
Save the file and close it to return to the terminal.
Note, you can also create your own warehouse using Gazebo’s drag and drop interface. Gazebo enables you to add pieces to the world and then save the world as an sdf file.
Test Your Warehouse
Now let’s run Gazebo so that we can see our model. Type the following command:
gazebo
On the left-hand side, click the “Insert” tab.
On the left panel, click “Small Warehouse”. You should see a warehouse robot. You can place it wherever you want by clicking inside the environment.
Click on your model to select it. I have also added our mobile warehouse robot to the scene.

Go back to the terminal window, and type CTRL + C to close Gazebo.
Launch Your Robot and Warehouse Using ROS 2
Now that we have created our robot and our warehouse, let’s see how we can launch these pieces using ROS 2.
Create a Package
Let’s create a package.
Open a new terminal window, and navigate to the src directory of your workspace (the name of my workspace is dev_ws, but your workspace might have a different name):
cd ~/dev_ws/src
Now let’s create a package named warehouse_robot_spawner_pkg.
ros2 pkg create --build-type ament_python warehouse_robot_spawner_pkg
Package Setup
Now that we have our package, we need to make sure some key files are in there.
Move inside the folder, and see what you have so far.
cd ~/dev_ws/src/warehouse_robot_spawner_pkg
dir

Open your package.xml file.
gedit package.xml
Fill in the description, your email address, and any license you want to use. Then save and close the file.

Now, open your setup.py file. Copy and paste these lines into it:
gedit setup.py
Save the file, and close it.
Now, we need to add our models. Create a new folder inside the ~/dev_ws/src/warehouse_robot_spawner_pkg directory named models.
mkdir ~/dev_ws/src/warehouse_robot_spawner_pkg/models
Go to your Gazebo models.
cd ~/.gazebo/models
Copy your models over to your package.
cp -r small_warehouse ~/dev_ws/src/warehouse_robot_spawner_pkg/models
cp -r mobile_warehouse_robot ~/dev_ws/src/warehouse_robot_spawner_pkg/models
Move the meshes over as well.
cp -r hokuyo ~/dev_ws/src/warehouse_robot_spawner_pkg/models
cp -r warehouse_robot ~/dev_ws/src/warehouse_robot_spawner_pkg/models
Check that everything copied over correctly.
cd ~/dev_ws/src/warehouse_robot_spawner_pkg/models
dir

Now let’s create a folder named worlds. We want to create an sdf file in here that handles the generation of the Gazebo environment and the warehouse.
mkdir ~/dev_ws/src/warehouse_robot_spawner_pkg/worlds
Move inside this folder.
cd ~/dev_ws/src/warehouse_robot_spawner_pkg/worlds
Open up a new file called warehouse.world. Even though this file has the .world extension, it is still an sdf file.
gedit warehouse.world
Add this code inside the file, click Save, then close it.
Create a Node
Now we need to create a ROS 2 node that will spawn the warehouse robot and connect it to ROS 2.
Move inside the warehouse_robot_spawner_pkg folder.
cd ~/dev_ws/src/warehouse_robot_spawner_pkg/warehouse_robot_spawner_pkg/
Open a new Python file named spawn_demo.py.
gedit spawn_demo.py
Write this code inside the file.
Save and close the Python program.
Create a Launch File
Now let’s create a launch file.
mkdir ~/dev_ws/src/warehouse_robot_spawner_pkg/launch/
cd ~/dev_ws/src/warehouse_robot_spawner_pkg/launch/
Create a file named gazebo_world.launch.py
gedit gazebo_world.launch.py
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 | # Copyright 2019 Open Source Robotics Foundation, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. """ Demo for spawn_entity. Launches Gazebo and spawns a model """ # A bunch of software packages that are needed to launch ROS2 import os from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import ThisLaunchFileDir,LaunchConfiguration from launch_ros.actions import Node from launch.actions import ExecuteProcess from ament_index_python.packages import get_package_share_directory def generate_launch_description(): use_sim_time = LaunchConfiguration( 'use_sim_time' , default = 'True' ) world_file_name = 'warehouse.world' pkg_dir = get_package_share_directory( 'warehouse_robot_spawner_pkg' ) os.environ[ "GAZEBO_MODEL_PATH" ] = os.path.join(pkg_dir, 'models' ) world = os.path.join(pkg_dir, 'worlds' , world_file_name) launch_file_dir = os.path.join(pkg_dir, 'launch' ) gazebo = ExecuteProcess( cmd = [ 'gazebo' , '--verbose' , world, '-s' , 'libgazebo_ros_init.so' , '-s' , 'libgazebo_ros_factory.so' ], output = 'screen' ) #GAZEBO_MODEL_PATH has to be correctly set for Gazebo to be able to find the model #spawn_entity = Node(package='gazebo_ros', node_executable='spawn_entity.py', # arguments=['-entity', 'demo', 'x', 'y', 'z'], # output='screen') spawn_entity = Node(package = 'warehouse_robot_spawner_pkg' , executable = 'spawn_demo' , arguments = [ 'WarehouseBot' , 'demo' , '-1.5' , '-4.0' , '0.0' ], output = 'screen' ) return LaunchDescription([ gazebo, spawn_entity, ]) |
Save the file and close it.
Build the Package
Return to the root of your workspace:
cd ~/dev_ws/
Make sure the setuptools package is installed.
sudo pip3 install setuptools
or
sudo pip install setuptools
Build the package.
colcon build --packages-select warehouse_robot_spawner_pkg
Launch
Ok, now we are ready to launch.
Open a new terminal window.
Move to the root of the workspace.
cd ~/dev_ws/
Type the following command:
ros2 launch warehouse_robot_spawner_pkg gazebo_world.launch.py

Here is the console output.

Let’s see what topics are active. Open a new terminal, and type:
ros2 topic list -t

Move the Robot Around the Warehouse
Now that we know how to spawn the robot and the world, let’s see how we can make the robot move using ROS 2.
I will make the mobile robot patrol the warehouse. It will use its laser range finder to follow walls.
Create a Package
Let’s create a package.
Open a new terminal window, and navigate to the src directory of your workspace:
cd ~/dev_ws/src
Now let’s create a package named warehouse_robot_controller_pkg.
ros2 pkg create --build-type ament_python warehouse_robot_controller_pkg
Package Setup
Now that we have our package, we need to make sure some important files are in there.
Move inside the folder, and see what you have so far.
cd ~/dev_ws/src/warehouse_robot_controller_pkg
dir
Open your package.xml file.
gedit package.xml

Fill in the description, your email address, and any license you want to use. Then save and close the file.
Now, open your setup.py file. Make sure it looks like this.
gedit setup.py
Save the file, and close it.
Create an Estimator and a Controller Node
Now we need to create two ROS 2 nodes. One node will be responsible for estimating the current state of the robot in the world (i.e. position and orientation). The other node will send velocity commands to the robot.
Move inside the warehouse_robot_controller_pkg folder.
cd ~/dev_ws/src/warehouse_robot_controller_pkg/warehouse_robot_controller_pkg/
Open a new Python file named robot_estimator.py. Don’t be intimidated by how much code there is. Just take it one section at a time. I included a lot of comments so that you know what is going on.
gedit robot_estimator.py
Write the following code inside the file.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 | # Author: Addison Sears-Collins # Date: March 19, 2021 # ROS Version: ROS 2 Foxy Fitzroy # Python math library import math # ROS client library for Python import rclpy # Used to create nodes from rclpy.node import Node # Twist is linear and angular velocity from geometry_msgs.msg import Twist # Position, orientation, linear velocity, angular velocity from nav_msgs.msg import Odometry # Handles laser distance scan to detect obstacles from sensor_msgs.msg import LaserScan # Used for laser scan from rclpy.qos import qos_profile_sensor_data # Enable use of std_msgs/Float64MultiArray message from std_msgs.msg import Float64MultiArray # Scientific computing library for Python import numpy as np class Estimator(Node): """ Class constructor to set up the node """ def __init__( self ): ############## INITIALIZE ROS PUBLISHERS AND SUBSCRIBERS ###### super ().__init__( 'Estimator' ) # Create a subscriber # This node subscribes to messages of type # nav_msgs/Odometry (i.e. position and orientation of the robot) self .odom_subscriber = self .create_subscription( Odometry, '/demo/odom' , self .odom_callback, 10 ) # Create a subscriber # This node subscribes to messages of type # geometry_msgs/Twist.msg. We are listening to the velocity commands here. # The maximum number of queued messages is 10. self .velocity_subscriber = self .create_subscription( Twist, '/demo/cmd_vel' , self .velocity_callback, 10 ) # Create a publisher # This node publishes the estimated position (x, y, yaw) # The type of message is std_msgs/Float64MultiArray self .publisher_state_est = self .create_publisher( Float64MultiArray, '/demo/state_est' , 10 ) def odom_callback( self , msg): """ Receive the odometry information containing the position and orientation of the robot in the global reference frame. The position is x, y, z. The orientation is a x,y,z,w quaternion. """ roll, pitch, yaw = self .euler_from_quaternion( msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w) obs_state_vector_x_y_yaw = [msg.pose.pose.position.x,msg.pose.pose.position.y,yaw] # Publish the estimated state (x position, y position, yaw angle) self .publish_estimated_state(obs_state_vector_x_y_yaw) def publish_estimated_state( self , state_vector_x_y_yaw): """ Publish the estimated pose (position and orientation) of the robot to the '/demo/state_est' topic. :param: state_vector_x_y_yaw [x, y, yaw] x is in meters, y is in meters, yaw is in radians """ msg = Float64MultiArray() msg.data = state_vector_x_y_yaw self .publisher_state_est.publish(msg) def euler_from_quaternion( self , x, y, z, w): """ Convert a quaternion into euler angles (roll, pitch, yaw) roll is rotation around x in radians (counterclockwise) pitch is rotation around y in radians (counterclockwise) yaw is rotation around z in radians (counterclockwise) """ t0 = + 2.0 * (w * x + y * z) t1 = + 1.0 - 2.0 * (x * x + y * y) roll_x = math.atan2(t0, t1) t2 = + 2.0 * (w * y - z * x) t2 = + 1.0 if t2 > + 1.0 else t2 t2 = - 1.0 if t2 < - 1.0 else t2 pitch_y = math.asin(t2) t3 = + 2.0 * (w * z + x * y) t4 = + 1.0 - 2.0 * (y * y + z * z) yaw_z = math.atan2(t3, t4) return roll_x, pitch_y, yaw_z # in radians def velocity_callback( self , msg): """ Listen to the velocity commands (linear forward velocity in the x direction in the robot's reference frame and angular velocity (yaw rate) around the robot's z-axis. [v,yaw_rate] [meters/second, radians/second] """ # Forward velocity in the robot's reference frame v = msg.linear.x # Angular velocity around the robot's z axis yaw_rate = msg.angular.z def main(args = None ): """ Entry point for the program. """ # Initialize rclpy library rclpy.init(args = args) # Create the node estimator = Estimator() # Spin the node so the callback function is called. # Pull messages from any topics this node is subscribed to. # Publish any pending messages to the topics. rclpy.spin(estimator) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) estimator.destroy_node() # Shutdown the ROS client library for Python rclpy.shutdown() if __name__ = = '__main__' : main() |
Save and close the Python program.
Open a new Python file named robot_controller.py.
gedit robot_controller.py
Write the following code inside the file.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 | # Author: Addison Sears-Collins # Date: March 19, 2021 # ROS Version: ROS 2 Foxy Fitzroy ############## IMPORT LIBRARIES ################# # Python math library import math # ROS client library for Python import rclpy # Enables pauses in the execution of code from time import sleep # Used to create nodes from rclpy.node import Node # Enables the use of the string message type from std_msgs.msg import String # Twist is linear and angular velocity from geometry_msgs.msg import Twist # Handles LaserScan messages to sense distance to obstacles (i.e. walls) from sensor_msgs.msg import LaserScan # Handle Pose messages from geometry_msgs.msg import Pose # Handle float64 arrays from std_msgs.msg import Float64MultiArray # Handles quality of service for LaserScan data from rclpy.qos import qos_profile_sensor_data # Scientific computing library import numpy as np class Controller(Node): """ Create a Controller class, which is a subclass of the Node class for ROS2. """ def __init__( self ): """ Class constructor to set up the node """ ##################### ROS SETUP #################################################### # Initiate the Node class's constructor and give it a name super ().__init__( 'Controller' ) # Create a subscriber # This node subscribes to messages of type Float64MultiArray # over a topic named: /demo/state_est # The message represents the current estimated state: # [x, y, yaw] # The callback function is called as soon as a message # is received. # The maximum number of queued messages is 10. self .subscription = self .create_subscription( Float64MultiArray, '/demo/state_est' , self .state_estimate_callback, 10 ) self .subscription # prevent unused variable warning # Create a subscriber # This node subscribes to messages of type # sensor_msgs/LaserScan self .scan_subscriber = self .create_subscription( LaserScan, '/demo/laser/out' , self .scan_callback, qos_profile = qos_profile_sensor_data) # Create a publisher # This node publishes the desired linear and angular velocity of the robot (in the # robot chassis coordinate frame) to the /demo/cmd_vel topic. Using the diff_drive # plugin enables the robot model to read this /demo/cmd_vel topic and execute # the motion accordingly. self .publisher_ = self .create_publisher( Twist, '/demo/cmd_vel' , 10 ) # Initialize the LaserScan sensor readings to some large value # Values are in meters. self .left_dist = 999999.9 # Left self .leftfront_dist = 999999.9 # Left-front self .front_dist = 999999.9 # Front self .rightfront_dist = 999999.9 # Right-front self .right_dist = 999999.9 # Right ################### ROBOT CONTROL PARAMETERS ################## # Maximum forward speed of the robot in meters per second # Any faster than this and the robot risks falling over. self .forward_speed = 0.025 # Current position and orientation of the robot in the global # reference frame self .current_x = 0.0 self .current_y = 0.0 self .current_yaw = 0.0 ############# WALL FOLLOWING PARAMETERS ####################### # Finite states for the wall following mode # "turn left": Robot turns towards the left # "search for wall": Robot tries to locate the wall # "follow wall": Robot moves parallel to the wall self .wall_following_state = "turn left" # Set turning speeds (to the left) in rad/s # These values were determined by trial and error. self .turning_speed_wf_fast = 3.0 # Fast turn self .turning_speed_wf_slow = 0.05 # Slow turn # Wall following distance threshold. # We want to try to keep within this distance from the wall. self .dist_thresh_wf = 0.50 # in meters # We don't want to get too close to the wall though. self .dist_too_close_to_wall = 0.19 # in meters def state_estimate_callback( self , msg): """ Extract the position and orientation data. This callback is called each time a new message is received on the '/demo/state_est' topic """ # Update the current estimated state in the global reference frame curr_state = msg.data self .current_x = curr_state[ 0 ] self .current_y = curr_state[ 1 ] self .current_yaw = curr_state[ 2 ] # Command the robot to keep following the wall self .follow_wall() def scan_callback( self , msg): """ This method gets called every time a LaserScan message is received on the '/demo/laser/out' topic """ # Read the laser scan data that indicates distances # to obstacles (e.g. wall) in meters and extract # 5 distinct laser readings to work with. # Each reading is separated by 45 degrees. # Assumes 181 laser readings, separated by 1 degree. # (e.g. -90 degrees to 90 degrees....0 to 180 degrees) #number_of_laser_beams = str(len(msg.ranges)) self .left_dist = msg.ranges[ 180 ] self .leftfront_dist = msg.ranges[ 135 ] self .front_dist = msg.ranges[ 90 ] self .rightfront_dist = msg.ranges[ 45 ] self .right_dist = msg.ranges[ 0 ] def follow_wall( self ): """ This method causes the robot to follow the boundary of a wall. """ # Create a geometry_msgs/Twist message msg = Twist() msg.linear.x = 0.0 msg.linear.y = 0.0 msg.linear.z = 0.0 msg.angular.x = 0.0 msg.angular.y = 0.0 msg.angular.z = 0.0 # Logic for following the wall # >d means no wall detected by that laser beam # <d means an wall was detected by that laser beam d = self .dist_thresh_wf if self .leftfront_dist > d and self .front_dist > d and self .rightfront_dist > d: self .wall_following_state = "search for wall" msg.linear.x = self .forward_speed msg.angular.z = - self .turning_speed_wf_slow # turn right to find wall elif self .leftfront_dist > d and self .front_dist < d and self .rightfront_dist > d: self .wall_following_state = "turn left" msg.angular.z = self .turning_speed_wf_fast elif ( self .leftfront_dist > d and self .front_dist > d and self .rightfront_dist < d): if ( self .rightfront_dist < self .dist_too_close_to_wall): # Getting too close to the wall self .wall_following_state = "turn left" msg.linear.x = self .forward_speed msg.angular.z = self .turning_speed_wf_fast else : # Go straight ahead self .wall_following_state = "follow wall" msg.linear.x = self .forward_speed elif self .leftfront_dist < d and self .front_dist > d and self .rightfront_dist > d: self .wall_following_state = "search for wall" msg.linear.x = self .forward_speed msg.angular.z = - self .turning_speed_wf_slow # turn right to find wall elif self .leftfront_dist > d and self .front_dist < d and self .rightfront_dist < d: self .wall_following_state = "turn left" msg.angular.z = self .turning_speed_wf_fast elif self .leftfront_dist < d and self .front_dist < d and self .rightfront_dist > d: self .wall_following_state = "turn left" msg.angular.z = self .turning_speed_wf_fast elif self .leftfront_dist < d and self .front_dist < d and self .rightfront_dist < d: self .wall_following_state = "turn left" msg.angular.z = self .turning_speed_wf_fast elif self .leftfront_dist < d and self .front_dist > d and self .rightfront_dist < d: self .wall_following_state = "search for wall" msg.linear.x = self .forward_speed msg.angular.z = - self .turning_speed_wf_slow # turn right to find wall else : pass # Send velocity command to the robot self .publisher_.publish(msg) def main(args = None ): # Initialize rclpy library rclpy.init(args = args) # Create the node controller = Controller() # Spin the node so the callback function is called # Pull messages from any topics this node is subscribed to # Publish any pending messages to the topics rclpy.spin(controller) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) controller.destroy_node() # Shutdown the ROS client library for Python rclpy.shutdown() if __name__ = = '__main__' : main() |
Save and close the Python program.
Create a Launch File
Now let’s create a launch file.
mkdir ~/dev_ws/src/warehouse_robot_controller_pkg/launch/
cd ~/dev_ws/src/warehouse_robot_controller_pkg/launch/
Create a file named controller_estimator.launch.py
gedit controller_estimator.launch.py
Add the following code.
1 2 3 4 5 6 7 8 9 10 11 12 13 | import os from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node(package = 'warehouse_robot_controller_pkg' , executable = 'robot_controller' , output = 'screen' ), Node(package = 'warehouse_robot_controller_pkg' , executable = 'robot_estimator' , output = 'screen' ), ]) |
Save the file and close it.
Build the Package
Return to the root of your workspace:
cd ~/dev_ws/
Build the package.
colcon build --packages-select warehouse_robot_controller_pkg
Launch
Ok, now we are ready to launch.
Open a new terminal window.
Move to the root of the workspace.
cd ~/dev_ws/
Type the following command to launch the gazebo world.
ros2 launch warehouse_robot_spawner_pkg gazebo_world.launch.py
Open a new terminal window, and launch the controller.
ros2 launch warehouse_robot_controller_pkg controller_estimator.launch.py
The robot will struggle at times around corners, but that is fine.

Let’s see what topics are active. Open a new terminal, and type:
ros2 topic list -t

Moving the Robot Around Manually
By the way, if you ever want to move the robot around manually using the keyboard and already have the turtlebot3 package installed, you can use the following commands.
Open a new terminal window, and type:
ros2 launch warehouse_robot_spawner_pkg gazebo_world.launch.py
Then open another terminal window, and type:
ros2 run turtlebot3_teleop teleop_keyboard --ros-args --remap /cmd_vel:=/demo/cmd_vel
The command will remap commands sent from the keyboard to the /cmd_vel topic to the /demo/cmd_vel topic (which is the topic that the robot gets its velocity commands from as you saw in the sdf file).
To install the Turtlebot3 package, you would need to use the following command:
sudo apt install ros-foxy-turtlebot3*
That’s it. Keep building!