How To Convert Euler Angles to Quaternions Using Python

Given Euler angles of the following form….

Rotation about the x axis = roll angle = α
Rotation about the y-axis = pitch angle = β
Rotation about the z-axis = yaw angle = γ

…how do we convert this into a quaternion of the form (x, y, z, w) where w is the scalar (real) part and x, y, and z are the vector parts?

Doing this operation is important because ROS2 (and ROS) uses quaternions as the default representation for the orientation of a robot in 3D space.

Here is the Python code:

#! /usr/bin/env python3

# This program converts Euler angles to a quaternion.
# Author: AutomaticAddison.com

import numpy as np # Scientific computing library for Python

def get_quaternion_from_euler(roll, pitch, yaw):
  """
  Convert an Euler angle to a quaternion.
  
  Input
    :param roll: The roll (rotation around x-axis) angle in radians.
    :param pitch: The pitch (rotation around y-axis) angle in radians.
    :param yaw: The yaw (rotation around z-axis) angle in radians.

  Output
    :return qx, qy, qz, qw: The orientation in quaternion [x,y,z,w] format
  """
  qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
  qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
  qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
  qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)

  return [qx, qy, qz, qw]

Example

Suppose a robot is on a flat surface. It has the following Euler angles:

Euler Angle (roll, pitch, yaw) = (0.0, 0.0, π/2) = (0.0, 0.0, 1.5708)

What is the robot’s orientation in quaternion format (x, y, z, w)?

The program shows that the quaternion is:

Quaternion [x,y,z,w] = [0, 0, 0.7071, 0.7071]

And that’s all there is to it folks. That’s how you convert a Euler angles into a quaternion.

How to Navigate With Keepout Zones – ROS 2 Navigation

In this tutorial, I will show you how to navigate using keepout zones using the ROS 2 Navigation Stack (also known as Nav2). A keepout zone is an area where a robot can’t enter. Here is the final output you will be able to achieve after going through this tutorial:

Real-World Applications

The application that we will develop in this tutorial can be used in a number of real-world robotic applications:

Hospitals and Medical Centers
Hotels (e.g. Room Service)
House
Offices
Restaurants
Warehouses
And more…

We will focus on creating an application that will prevent a robot from entering specific locations on a warehouse or factory floor.

Prerequisites

ROS 2 Foxy Fitzroy installed on Ubuntu Linux 20.04 or newer. I am using ROS 2 Galactic, which is the latest version of ROS 2 as of the date of this post.
You have already created a ROS 2 workspace. The name of our workspace is “dev_ws”, which stands for “development workspace.”
You have Python 3.7 or higher.
(Optional) You have completed my Ultimate Guide to the ROS 2 Navigation Stack.
(Optional) You have a package named two_wheeled_robot inside your ~/dev_ws/src folder, which I set up in this post. If you have another package, that is fine.
(Optional) You know how to load a world file into Gazebo using ROS 2.

You can find the files for this post here on my Google Drive. Credit to this GitHub repository for the code.

Create a World

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/worlds

Make sure this world is inside this folder. The name of the file is warehouse_keepout_zones.world.

Create a Map

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/maps/warehouse_world

Make sure the pgm and yaml map files are inside this folder.

My world map is made up of two files:

warehouse_world_keepout_zones.pgm
warehouse_world_keepout_zones.yaml

Create a Filter Mask

Now we need to create a mask that identifies the keepout zones.

This tutorial has instructions on how to use a graphics editor like GIMP (you can install using the sudo apt-get install gimp command) to prepare the filter mask.

You will start with a copy of the world file you want to use. In this tutorial, I am going to use my warehouse_world_keepout_zones.pgm file. You need to edit this file so that keepout zones are black.

My filter mask is made up of two files:

keepout_rotated2.pgm
keepout2.yaml

Both of these files are located in my ~/dev_ws/src/two_wheeled_robot/maps/warehouse_world/ folder.

Create the Parameters File

Let’s create the parameters file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/params/warehouse_world

Add this file. The name of the file is keepout_zones_params.yaml.

Create the RViz Configuration File

Let’s create the RViz configuration file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/rviz/warehouse_world

Add this file. The name of the file is nav2_config_keepout_zones.rviz.

Create the Launch File

Let’s create the launch file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/launch/warehouse_world

Add this file. The name of the file is warehouse_world_keepout_zones.launch.py.

Launch the Launch File

We will now build our package.

cd ~/dev_ws/

colcon build

Open a new terminal and launch the robot in a Gazebo world.

ros2 launch two_wheeled_robot warehouse_world_keepout_zones.launch.py

Wait for the robot to snap to the estimated initial location within RViz. This process should take a minute or two.

You might notice that the robot’s pose in RViz is not aligned with the pose in Gazebo. Localization using the AMCL (Adaptive Monte Carlo Localization) package is really sensitive to the initial pose estimate. The trick is to make sure the initial location of the robot is in a location with a lot of distinctively shaped obstacles (i.e. the shelves and boxes) for the LIDAR to read.

Even though the initial pose was set when we launched the robot, it is likely that the estimate in RViz is pretty bad. Let’s set the initial pose again by clicking the 2D Pose Estimate button at the top of RViz and then clicking on the map.

You can also set the initial pose by opening a new terminal window and typing the following command.

ros2 topic pub -1 /initialpose geometry_msgs/PoseWithCovarianceStamped '{ header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, pose: { pose: {position: {x: -3.7, y: 9.0, z: 0.0}, orientation: {w: 1.0}}, } }'

When the robot snaps to the location, the map and odom axes should be pretty close to each other right at the origin of the map (x=0, y=0).

Now send the robot to a goal that is on the other side of the keepout zones by clicking the “Nav2 Goal” button at the top of RViz and clicking on a goal location.

The robot will move to the goal, avoiding the keepout zone along the way.

Your robot might stop and abort the missions. The reason for this is the lack of space between keepout zones. If this occurs, relaunch the launch file and try choosing a different navigation goal. You can also the rqt_robot_steering terminal command to drive the robot to a different initial pose (i.e. just type rqt_robot_steering in a fresh terminal window).

A success message will print once the robot has reached the goal location.

That’s it! Keep building!

How to Navigate Within Preferred Lanes – ROS 2 Navigation

In this tutorial, I will show you how to navigate using preferred lanes using the ROS 2 Navigation Stack (also known as Nav2). Here is the final output you will be able to achieve after going through this tutorial:

Real-World Applications

The application that we will develop in this tutorial can be used in a number of real-world robotic applications:

Hospitals and Medical Centers
Hotels (e.g. Room Service)
House
Offices
Restaurants
Warehouses
And more…

We will focus on creating an application that will enable an autonomous mobile robot to move along pre-specified paths on a warehouse or factory floor.

Prerequisites

ROS 2 Foxy Fitzroy installed on Ubuntu Linux 20.04 or newer. I am using ROS 2 Galactic, which is the latest version of ROS 2 as of the date of this post.
You have already created a ROS 2 workspace. The name of our workspace is “dev_ws”, which stands for “development workspace.”
You have Python 3.7 or higher.
(Optional) You have completed my Ultimate Guide to the ROS 2 Navigation Stack.
(Optional) You have a package named two_wheeled_robot inside your ~/dev_ws/src folder, which I set up in this post. If you have another package, that is fine.
(Optional) You know how to load a world file into Gazebo using ROS 2.

You can find the files for this post here on my Google Drive. Credit to this GitHub repository for the code.

Create a World

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/worlds

Make sure this world is inside this folder. The name of the file is warehouse_preferred_lanes.world.

Create a Map

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/maps/warehouse_world

Make sure the pgm and yaml map files are inside this folder.

My world map is made up of two files:

warehouse_world_preferred_lanes.pgm
warehouse_world_preferred_lanes.yaml

Create a Filter Mask

Now we need to create a mask that identifies the preferred lanes.

This tutorial has instructions on how to use a graphics editor like GIMP (you can install using the sudo apt-get install gimp command) to prepare the filter mask.

You will start with a copy of the world file you want to use. In this tutorial, I am going to use my warehouse_world_preferred_lanes.pgm file. You need to edit this file so that free travel lanes are white and keepout zones are black.

My filter mask is made up of two files:

lanes_rotated.pgm
lanes.yaml

Both of these files are located in my ~/dev_ws/src/two_wheeled_robot/maps/warehouse_world/ folder.

Create the Parameters File

Let’s create the parameters file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/params/warehouse_world

Add this file (preferred_lanes_params.yaml).

Create the RViz Configuration File

Let’s create the RViz configuration file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/rviz/warehouse_world

Add this file (nav2_config_preferred_lanes.rviz).

Create the Launch File

Let’s create the launch file.

Open a terminal window, and move to your package.

cd ~/dev_ws/src/two_wheeled_robot/launch/warehouse_world

Add this file (warehouse_world_preferred_lanes.launch.py).

Launch the Launch File

We will now build our package.

cd ~/dev_ws/

colcon build

Open a new terminal and launch the robot in a Gazebo world.

ros2 launch two_wheeled_robot warehouse_world_preferred_lanes.launch.py

Wait for the robot to snap to the estimated initial location within RViz. This process should take a minute or two.

You can also set the initial pose by opening a new terminal window and typing the following command (all of this stuff below is a single command).

ros2 topic pub -1 /initialpose geometry_msgs/PoseWithCovarianceStamped '{ header: {stamp: {sec: 0, nanosec: 0}, frame_id: "map"}, pose: { pose: {position: {x: -3.7, y: 9.0, z: 0.0}, orientation: {w: 1.0}}, } }'

When the robot snaps to the location, the map and odom axes should be pretty close to each other right at the origin of the map (x=0, y=0).

Now send the robot to a goal that is inside the preferred lane (i.e. the white space) by clicking the “Nav2 Goal” button at the top of RViz and clicking on a goal location.

The robot will move to the goal, following the lane along the way.

A success message will print in the terminal window once the robot has reached the goal location.

That’s it! Keep building!