Category: Robotics

Setting Up the ROS Navigation Stack for a Simulated Robot

In this tutorial, I will show you how to set up the ROS Navigation Stack for a robot in simulation.

Credit to Ramkumar Gandhinathan and Lentin Joseph’s awesome book ROS Robotics Projects Second Edition (Disclosure: As an Amazon Associate I earn from qualifying purchases). I highly recommend it if you want to learn ROS 1. Many of the files in this tutorial (URDF, configuration, and STL files), come from their book’s public GitHub page.

What is the ROS Navigation Stack?

The ROS Navigation Stack is a collection of software packages that you can use to help your robot move from a starting location to a goal location safely.

The official steps for setup and configuration are at this link on the ROS website, but we will walk through everything together, step-by-step, because those instructions leave out a lot of detail. 

IMPORTANT: For your reference, all our code will be located in this folder, which I named simulated_home_delivery_robot.

Real-World Applications

This project has a number of real-world applications: 

  • Indoor and Outdoor Delivery Robots
  • Room Service Robots
  • Robot Vacuums
  • Order Fulfillment
  • Manufacturing
  • Factories

We will create a robot that will exist in a world that contains a post office and three houses. The use case for this simulated robot would be picking up packages at a post office and delivering them to houses in a neighborhood. 

Let’s get started!

Prerequisites

Create a ROS Package

Let’s create a ROS package.

In a new terminal window, move to the src (source) folder of your workspace.

cd ~/catkin_ws/src

Now create the package.

catkin_create_pkg mobile_manipulator actionlib roscpp rospy std_msgs
cd ~/catkin_ws/
catkin_make --only-pkg-with-deps mobile_manipulator

Set Up the World

Create a new folder called worlds inside the mobile_manipulator package.

roscd mobile_manipulator
mkdir worlds
cd worlds

Open a new file called postoffice.world.

gedit postoffice.world

Download, and then add this code.

Save the file and close it.

Let’s visualize this world using Gazebo.

gazebo postoffice.world

On my computer, this file took five minutes to load, so be patient. 

2-simulated-post-office-world-gazebo

When you want to close Gazebo, type CTRL + C in all terminal windows to close everything down.

Make a note of the location of this world file. We’ll need it later. Type:

pwd

The path on my computer is:

/home/focalfossa/catkin_ws/src/mobile_manipulator/worlds/postoffice.world

Now open a new terminal window, and type:

roscd mobile_manipulator
mkdir launch

Now, go get your launch file from this tutorial. The name of my launch file is mobile_manipulator_gazebo.launch. I can access this file by opening a new terminal window, and typing:

roscd mobile_manipulator_body/launch/

Take that launch file and put it in the following directory:

roscd mobile_manipulator/launch/

Within the ~/mobile_manipulator/launch/ directory, open the launch file.

gedit mobile_manipulator_gazebo.launch

Your launch file should look like the following. You will notice that we added the post office world file.

Save the file, and close it.

Launch the Robot in the Gazebo World

Now let’s launch the robot in Gazebo.

Move to your catkin workspace.

cd ~/catkin_ws/
roslaunch mobile_manipulator mobile_manipulator_gazebo.launch

Here is my output:

4-robot-launched-in-the-world

Add a LIDAR

Create the Code

Now that we have set up the environment, let’s add a LIDAR to our robot so that it can perform SLAM (Simultaneous Localization And Mapping) within the post office world.

Open a new terminal window. Type the following command.

roscd mobile_manipulator_body/urdf/

Open mobile_manipulator.urdf.

gedit mobile_manipulator.urdf

Make sure your file has this code. Add the code between the ARM block and the BASE TRANSMISSIONS block.

You will see that we have added the LIDAR (i.e. laser_link), and we have also added a Gazebo plugin for it. 

Save and close the file.

Test the LIDAR

Now let’s launch the robot in Gazebo.

Open a new terminal window, and move to your catkin workspace.

cd ~/catkin_ws/
roslaunch mobile_manipulator mobile_manipulator_gazebo.launch

Here is my output:

8-add-lidar-sensor

To see the active coordinate frames, type the following command:

rosrun tf2_tools view_frames.py

To open the pdf file that shows the coordinate frames, type the following command:

evince frames.pdf

Configure the ROS Navigation Stack Parameters

Now that we have set up the world and added a LIDAR to our robot, we can set up and configure the ROS Navigation Stack so that our simulated robot can move autonomously through the environment. 

Open a new terminal window.

Move to your package.

roscd mobile_manipulator

Create a new folder. We want this folder to hold the configuration parameters. 

mkdir param
cd param

The ROS Navigation Stack uses two costmaps to store information about obstacles in the world. 

  1. Global costmap: This costmap is used to generate long term plans over the entire environment….for example, to calculate the shortest path from point A to point B on a map.
  2. Local costmap: This costmap is used to generate short term plans over the environment….for example, to avoid obstacles.

We have to configure these costmaps for our project. We set the configurations in .yaml files.

Common Configuration (Global and Local Costmap)

Let’s create a configuration file that will house parameters that are common to both the global and the local costmap. The name of this file will be costmap_common_params.yaml.

gedit costmap_common_params.yaml

Add the costmap_common_params.yaml code to this file.

Save and close the file.

To learn more about each of the parameters and what they mean, check out this link.

Global Configuration (Global Costmap)

Let’s create a configuration file that will house parameters for the global costmap. The name of this file will be global_costmap_params.yaml.

gedit global_costmap_params.yaml

Add the global_costmap_params.yaml code to this file.

Save and close the file.

Local Configuration (Local Costmap)

Let’s create a configuration file that will house parameters for the local costmap. The name of this file will be local_costmap_params.yaml.

gedit local_costmap_params.yaml

Add the local_costmap_params.yaml code to this file.

Save and close the file.

Base Local Planner Configuration

In addition to the costmap configurations we did in the previous section, we need to configure ROS Navigation Stack’s base local planner. The base_local_planner computes velocity commands that are sent to the robot base controller. The values that you use for your base_local_planner will depend on your robot.

gedit base_local_planner_params.yaml

Add the base_local_planner_params.yaml code to this file.

Save and close the file.

Configure RViz Settings

Add this file named mobile_manipulator.rviz to the param folder of your package.

roscd mobile_manipulator/param

Install the ROS Navigation Stack

Open a terminal window, and type the following command:

sudo apt-get install ros-noetic-navigation

To see if it installed correctly, type:

rospack find amcl

You should see the following output:

/opt/ros/noetic/share/amcl

Tune the AMCL Parameters

Open up your AMCL node.

roscd amcl/examples

Open the amcl_diff.launch file.

sudo gedit amcl_diff.launch

Here is my amcl_diff.launch file.

Set the following parameters, save the file, and close it.

<param name="odom_model_type" value="diff-corrected"/> <param name="odom_alpha1" value="0.005"/> <param name="odom_alpha2" value="0.005"/> <param name="odom_alpha3" value="0.010"/> <param name="odom_alpha4" value="0.005"/> <param name="odom_alpha5" value="0.003"/>

Press CTRL + C in all windows to close everything down.

Create a Map Using the ROS Hector-SLAM Package

Let’s create a map using the ROS Hector-SLAM package. For more information on this package, check this post.

Install Qt4

Install Qt4. Qt4 is a software that is used to generate graphical user interfaces.

sudo add-apt-repository ppa:rock-core/qt4
sudo apt update
sudo apt-get install qt4-qmake qt4-dev-tools

Type Y and press Enter to complete the installation. The whole process should take a while, so just be patient.

Download the Hector-SLAM Package

Move to your catkin workspace’s source folder.

cd ~/catkin_ws/src

Clone the Hector-SLAM package into your workspace.

git clone https://github.com/tu-darmstadt-ros-pkg/hector_slam.git

Set the Coordinate Frame Parameters

We need to set the frame names and options correctly.

Go to the launch file for Hector-SLAM. All of this below is a single command, so you can just copy and paste.

sudo gedit ~/catkin_ws/src/hector_slam/hector_mapping/launch/mapping_default.launch

Search for these lines (lines 5 and 6 in my code).

<arg name="base_frame" default="base_footprint"/> <arg name="odom_frame" default="nav"/>

Change those lines to this:

<arg name="base_frame" default="base_link"/> 
<arg name="odom_frame" default="odom"/>

Now go to the end of this file, and find these lines (line 54 in my code).

<node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser_link 100"/>

Save the file, and return to the terminal window.

Open a new terminal window, and type this command:

cd ~/catkin_ws/

Build the packages.

catkin_make --only-pkg-with-deps hector_slam

If you see this error message…

Project ‘cv_bridge’ specifies ‘/usr/include/opencv’ as an include dir, which is not found. It does neither exist as an absolute directory nor in…

Type:

cd /usr/include
sudo ln -s opencv4/ opencv

Build the packages again.

catkin_make --only-pkg-with-deps hector_slam

Wait a minute or two while the Hector-SLAM package builds.

Launch Mapping

Open a new terminal, and type the following command:

roslaunch mobile_manipulator mobile_manipulator_gazebo.launch

Now launch the mapping process.

roslaunch hector_slam_launch tutorial.launch

You can steer the robot by opening a new window and typing:

rosrun rqt_robot_steering rqt_robot_steering

Move the robot around the environment slowly. Map-making works best at slow (EXTREMELY slow) speeds. 

I recommend the following sequence of actions:

  • Move the robot backwards at 0.03 meters per second (i.e. -0.03 m/s). 
  • Turn the robot 90 degrees in-place at a rate of -0.24 radians per second.
  • Move the robot forward at a rate of 0.03 meters per second. 
  • Move the robot backwards at 0.03 meters per second.

Let’s save the map using a package called map_server. This package will save map data to a yaml and pgm formatted file.

Install map_server.

roscd mobile_manipulator/maps
sudo apt-get install ros-noetic-map-server

When you are happy with the map that you see in rviz, you can save the map as test.yaml and test.pgm. Open a new terminal, and type:

roscd mobile_manipulator/maps
rosrun map_server map_saver -f test

Maps will save to the ~…mobile_manipulator/maps directory.

Press CTRL + C on all terminal windows to shut everything down.

Load a Saved Map

To view the map, you can run the following command in a new terminal window to get the ROS Master started.

roscd mobile_manipulator/maps
roscore

Now load the map. In a new terminal window, type:

rosrun map_server map_server test.yaml

Open rviz in another terminal.

rviz

Click Add in the bottom left, and add the Map display.

Under Topic under the Map section, select /map.

You should see the saved map on your screen.

9d-create-a-map-hector-slam

Create a Preliminary Launch File

At this stage, we want to create a preliminary ROS launch file. This file will enable us to launch our mobile manipulator with the necessary mapping software as well as the move base and Adaptive Monte Carlo Localization (AMCL) nodes. You can learn more about the move base and AMCL nodes in this tutorial.

Type the following command.

roscd mobile_manipulator/launch

Create the following launch file.

gedit mobile_manipulator_move_base.launch

Add this code to the file.

Save the file, and close it. 

Add an Inertial Measurement Unit (IMU) to the Robot

Let’s add an IMU sensor to our robot. IMUs can help us get more accurate localization.

Open a new terminal window. Type the following command.

roscd mobile_manipulator_body/urdf/

Open mobile_manipulator.urdf.

gedit mobile_manipulator.urdf

Make sure your file has the IMU code inside the SENSORS block right after the code for the laser_link.

Save and close the file.

Test the IMU

Now let’s launch the robot in Gazebo.

Open a new terminal window, and move to your catkin workspace.

cd ~/catkin_ws/
roslaunch mobile_manipulator mobile_manipulator_gazebo.launch

Open a new terminal, and type:

rostopic list

You should see a topic named /imu_data.

Set Up the robot_pose_ekf Package

Now we will install the robot_pose_ekf package. This package uses an extended Kalman filter to help us estimate the position and orientation of the robot from sensor data. 

Getting Started

Open a new terminal window, and type:

sudo apt-get install ros-noetic-robot-pose-ekf

Now move to your workspace.

cd ~/catkin_ws

Build the package.

catkin_make

Add the the robot_pose_ekf node to a ROS Launch File

To launch the robot_pose_ekf node, you will need to add it to a launch file. Before we do that, let’s talk about the robot_pose_ekf node.

About the robot_pose_ekf Node

The robot_pose_ekf node will subscribe to the following topics (ROS message types are in parentheses):

This node will publish data to the following topics:

You might now be asking, how do we give the robot_ekf_pose node the data it needs? 

The data for /odom will come from the /base_truth_odom topic which is declared inside the URDF file for the robot.

The data for /imu_data  will come from the /imu_data topic which is also declared inside the URDF file for the robot. However, in this simulation, I will not use the IMU data since we are using Gazebo ground truth for the odometry. If you want to use the IMU data, you will set that parameter to true inside the launch file section for the robot_pose_ekf code.

In the launch file, we need to remap the data coming from the /base_truth_odom topic since the robot_pose_ekf node needs the topic name to be /odom.

Update the Launch File

Here is my full launch file. You will see that I have added static transform publishers at the top to get the data from base_footprint to base_link. 

Launch the Autonomous Mobile Robot!

Let’s test to see if autonomous navigation is working.

Now, open a new terminal window, and type:

roslaunch mobile_manipulator mobile_manipulator_gazebo.launch

Then in another terminal window, type:

roslaunch mobile_manipulator mobile_manipulator_move_base.launch

You set a pose estimate inside the map by clicking the 2D Pose Estimate button in Rviz and placing the estimate somewhere in line with where the robot currently is on the map.

10-2d-pose-estimate

Then you set the goal by clicking the Nav Goal button in Rviz and placing the goal in an empty space on the map. 

15-testing-autonomous-navigation-2

The path will be drawn as a red line, and the robot will attempt to move to the goal location due to the velocity commands published by the move_base node.

16e-post-office

To see the active coordinate frames, type the following command:

rosrun tf2_tools view_frames.py

To open the pdf file that shows the coordinate frames, type the following command:

evince frames.pdf

If you have difficulties, check out the differential drive control parameters in your control.yaml file. You can tweak the wheel_separation,  wheel_radius, and velocity parameters inside this file by typing the following command:

roscd mobile_manipulator_body/config
gedit control.yaml

You can also tweak the velocity settings in your base_local_planner_params.yaml file and the footprint in the costmap_common_params.yaml file.

That’s it. Keep building!

How to Create a Finite State Machine Using SMACH and ROS

In this tutorial, we will learn how to create a finite state machine using the SMACH Python-based library and ROS. 

Real-World Applications

In the context of robotics, a finite state machine consists of states (e.g. robot is OFF, robot is moving, robot is charging, etc.), an initial state, and inputs that can cause the robot system to change from one state to another state. 

Prerequisites

Finite State Machine Example

1-turnstile
Source: Wikipedia 

A turnstile at a metro station or a stadium is an example of a mechanism that can be modeled using a finite state machine. In the case of a turnstile, we have the following finite state machine:

2-fsm-diagram
Source: Wikipedia 
3-table

You will notice that there are two states: locked and unlocked. There are two inputs that can cause the current state to transition from one state to another: coin (i.e. customer inserts coin) and push (i.e. customer pushes the turnstile). 

In the locked state, when the customer inserts a coin, the turnstile transitions to unlocked. However, if the customer pushes the turnstile without inserting a coin, the turnstile remains locked.

In the unlocked state, when the customer inserts a coin, the turnstile remains in the unlocked state. If the customer pushes the turnstile in the unlocked state, the turnstile let’s the customer through, and then it transitions to the locked state.

Getting Started

To create finite state machines in ROS, we use the Python library called SMACH (you pronounce this as “smash”).

Let’s begin by installing the required software.

sudo apt-get update
sudo apt-get upgrade
sudo apt-get install ros-noetic-smach ros-noetic-smach-ros ros-noetic-executive-smach 

Create a ROS Package

Let’s create a ROS package.

In a new terminal window, move to the src (source) folder of your workspace.

cd ~/catkin_ws/src

Now create the package.

catkin_create_pkg turnstile_smach std_msgs roscpp rospy
cd ~/catkin_ws/
catkin_make --only-pkg-with-deps turnstile_smach

Write the Code

Move to your package by opening a new terminal window, and typing:

roscd turnstile_smach

Create a new folder called scripts.

mkdir scripts

Move inside that folder.

cd scripts

Create a new script.

gedit fsm_turnstile.py

Add the following code inside the script.

#!/usr/bin/env python3

# Author: Automatic Addison https://automaticaddison.com
# Description: An example of a basic finite state machine for a turnstile at 
#   a stadium or metro station.

# Import the necessary libraries
import rospy # Python client library
from smach import State, StateMachine # State machine library
import smach_ros # Extensions for SMACH library to integrate it with ROS
from time import sleep # Handle time

# Define state LOCKED
class Locked(State):
  def __init__(self):
    State.__init__(self, outcomes=['push','coin'], input_keys=['input'])

  # Inside this block, you can execute any code you want
  def execute(self, userdata):
    sleep(1)
    
    rospy.loginfo('Executing state LOCKED')
		
    # When a state finishes, an outcome is returned. An outcome is a 
    # user-defined string that describes how a state finishes.
    # The transition to the next state is based on this outcome
    if userdata.input == 1:
      return 'push'
    else:
      return 'coin'

# Define state UNLOCKED
class Unlocked(State):
  def __init__(self):
    State.__init__(self, outcomes=['push','coin'], input_keys=['input'])

  def execute(self, userdata):
    sleep(1)

    rospy.loginfo('Executing state UNLOCKED')

    if userdata.input == 1:
      return 'push'
    else:
      return 'coin'

# Main method
def main():

  # Initialize the node
  rospy.init_node('fsm_turnstile_py')

  # Create a SMACH state machine container
  sm = StateMachine(outcomes=['succeeded','failed'])

  # Set user data for the finite state machine
  sm.userdata.sm_input = 0
  #sm.userdata.sm_input = input("Enter 1 to Push or 0 to Insert a Coin: ")
	
  # Open the state machine container. A state machine container holds a number of states.
  with sm:
	
    # Add states to the container, and specify the transitions between states
    # For example, if the outcome of state LOCKED is 'coin', then we transition to state UNLOCKED.
    StateMachine.add('LOCKED', Locked(), transitions={'push':'LOCKED','coin':'UNLOCKED'}, remapping={'input':'sm_input'})
    StateMachine.add('UNLOCKED', Unlocked(), transitions={'push':'LOCKED','coin':'UNLOCKED'}, remapping={'input':'sm_input'})

  # View our state transitions using ROS by creating and starting the instrospection server
  sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
  sis.start()
  
  # Execute the state machine 
  outcome = sm.execute()

  # Wait for ctrl-c to stop the application
  rospy.spin()
  sis.stop()

if __name__ == '__main__':
  main()

Save the file, and close it.

Change the permissions of the file.

chmod +x fsm_turnstile.py

Open a new terminal window, and type:

cd ~/catkin_ws/

Build the package.

catkin_make --only-pkg-with-deps turnstile_smach

Run the Code

Open a new terminal window, and type:

cd ~/catkin_ws/

Start ROS.

roscore

Open a new terminal, and launch the finite state machine Python file you created.

rosrun turnstile_smach fsm_turnstyle.py

Here is the output:

4-smach-output

If you want to change the output, you can change the code so that the state machine input is 1 (i.e. push).

sm.userdata.sm_input = 1

Note that the SMACH viewer is unmaintained and doesn’t work with ROS Noetic.

That’s it! Keep building!

For a more in-depth look at SMACH with ROS, check out the official tutorials.

How to Move a Simulated Robot Arm to a Goal Using ROS

In this tutorial, we will learn how to use the ROS actionlib to send goals to a simulated robot arm. By the end of this tutorial, you will be able to build the following:

send-goals-to-arm

Real-World Applications

This project has a number of real-world applications: 

  • Order Fulfillment
  • Factories
  • Warehouses
  • Manufacturing
  • Kitchens
  • and more!

Let’s get started!

Prerequisites

  • You have completed this tutorial where you learned how to create a simulated robot arm.

Create a ROS Package

Let’s begin by creating a ROS package.

In a new terminal window, move to the src (source) folder of your workspace.

cd ~/catkin_ws/src

Now create the package.

catkin_create_pkg arm_to_goal actionlib roscpp rospy std_msgs
cd ~/catkin_ws/
catkin_make --only-pkg-with-deps arm_to_goal

Write the Code

Open a new terminal window.

Move to your package.

roscd arm_to_goal

Create a folder for the Python script we will write.

mkdir scripts

Move inside that folder.

cd scripts

Create a program.

gedit send_goal_to_arm.py

Add the following code:

#!/usr/bin/env python3

# Author: Automatic Addison https://automaticaddison.com
# Description: Uses the ROS action lib to move a robotic arm to a goal location

# Import the necessary libraries
from __future__ import print_function # Printing
import rospy # Python client library
import actionlib # ROS action library
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal # Controller messages
from std_msgs.msg import Float64 # 64-bit floating point numbers
from trajectory_msgs.msg import JointTrajectoryPoint # Robot trajectories


def move_robot_arm(joint_values):
  """
  Function to move the robot arm to desired joint angles.
  :param: joint_values A list of desired angles for the joints of a robot arm 
  """
  # Create the SimpleActionClient, passing the type of the action to the constructor
  arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)

  # Wait for the server to start up and start listening for goals.
  arm_client.wait_for_server()
	
  # Create a new goal to send to the Action Server
  arm_goal = FollowJointTrajectoryGoal()

  # Store the names of each joint of the robot arm
  arm_goal.trajectory.joint_names = ['arm_base_joint', 'shoulder_joint','bottom_wrist_joint' ,'elbow_joint', 'top_wrist_joint']
  
  # Create a trajectory point  	
  point = JointTrajectoryPoint()

  # Store the desired joint values
  point.positions = joint_values

  # Set the time it should in seconds take to move the arm to the desired joint angles
  point.time_from_start = rospy.Duration(3)

  # Add the desired joint values to the goal
  arm_goal.trajectory.points.append(point)

  # Define timeout values
  exec_timeout = rospy.Duration(10)
  prmpt_timeout = rospy.Duration(5)

  # Send a goal to the ActionServer and wait for the server to finish performing the action
  arm_client.send_goal_and_wait(arm_goal, exec_timeout, prmpt_timeout)


if __name__ == '__main__':
  """
  Main method.
  """
  try:
    # Initialize a rospy node so that the SimpleActionClient can
    # publish and subscribe over ROS.
    rospy.init_node('send_goal_to_arm_py')

    # Move the joints of the robot arm to the desired angles in radians
    move_robot_arm([-0.1, 0.5, 0.02, 0, 0])

    print("Robotic arm has successfully reached the goal!")
    
  except rospy.ROSInterruptException:
    print("Program interrupted before completion.", file=sys.stderr)

Change the permissions.

chmod +x send_goal_to_arm.py

Compile the package.

cd ~/catkin_ws/
catkin_make --only-pkg-with-deps arm_to_goal

Run the Code

Open a new terminal window, and type:

cd ~/catkin_ws/
roslaunch mobile_manipulator_body mobile_manipulator_gazebo.launch
1-robot-mobile-manipulator

Open a new terminal window, and type the following command to move the robot to the goal location:

rosrun arm_to_goal send_goal_to_arm.py
2-after-moving-1
3-action-completed

That’s it! Keep building!