Category: Robotics

Coordinate Frames and Transforms for ROS-based Mobile Robots

In this tutorial, we will cover the basics of the standard coordinate frames (i.e. x,y,z axes) for mobile robots that use ROS. We will also cover how to set up coordinate frames for a basic two-wheeled differential drive mobile robot.

Prerequisites

You know how to create a ROS launch file.

Why Coordinate Frames Are Important?

Suppose we have a LIDAR mounted on top of a wheeled robot like in the cover image of this tutorial. The job of the LIDAR is to provide data about the distances to objects in the environment.

You will notice how the LIDAR is not located at the center point of the robot. It is mounted slightly forward from the center of the robot…perhaps 10 cm from the center point of the robot’s base.

In a real-world scenario, in order for the robot to avoid obstacles as it moves around in the environment, we need to convert a detected object’s coordinates in the LIDAR coordinate frame to equivalent coordinates in the robot base’s frame. 

The LIDAR might detect an object at 100 cm away, for example…but what does that mean in terms of the robot base frame? If the LIDAR is mounted on the front part of the robot, 10 cm forward from the center point of the robot’s base frame, the object might actually be 110 cm away from the robot. 

You can see why coordinate frames and being able to transform data from one coordinate frame to another is important for accurate autonomous navigation. 

Check out the first part of this post at the official ROS website which has a good discussion of why coordinate frames are so important for a robot.

Standard Coordinate Frames in ROS for a Mobile Robot

Below are the standard coordinate frames for a basic two wheeled differential drive robot.

  • The red arrows represent the x axes
  • The blue arrows represent the z axes
  • The green dots (into the page) represent the y axes.
coordsystems_img
Image Source: ROS Website

The official ROS documents have an explanation of these coordinate frames, but let’s briefly define the main ones.

  • map frame has its origin at some arbitrarily chosen point in the world. This coordinate frame is fixed in the world.
  • odom frame has its origin at the point where the robot is initialized. This coordinate frame is fixed in the world.
  • base_footprint has its origin directly under the center of the robot. It is the 2D pose of the robot. This coordinate frame moves as the robot moves.
  • base_link has its origin directly at the pivot point or center of the robot. This coordinate frame moves as the robot moves.
  • laser_link has its origin at the center of the laser sensor (i.e. LIDAR). This coordinate frame remains fixed (i.e. “static”) relative to the base_link.

If you have other sensors on your robot, like an IMU, you can have a coordinate frame for that as well.

Static Transform Publisher

How can we automatically convert data that is published in one coordinate frame to equivalent data in another coordinate frame? For example, suppose we have an object at coordinate (x=3.7, y=1.23, z = 0.0) in the map coordinate frame. We want to navigate the robot to this object. What is the object’s position relative to the base_link coordinate frame?

Fortunately, ROS has a package called tf to handle all these coordinate transforms for us.

For coordinate frames that don’t change relative to each other through time (e.g. laser_link to base_link stays static because the laser is attached to the robot), we use the Static Transform Publisher. 

For coordinate frames that do change relative to each other through time (e.g. map to base_link), we use tf broadcasters and listeners. A lot of ROS packages handle these moving coordinate frame transforms for you, so you often don’t need to write these yourself.

Examples

coordinate_frames

Let’s take a look at some examples of how to set up static transform publishers in ROS. These static transform publishers will typically appear inside the launch file for whatever robot you’re working on.

Here is the syntax:

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms

From the ROS website: “Publish a static coordinate transform to tf using an x/y/z offset in meters and yaw/pitch/roll in radians. (yaw is rotation about Z, pitch is rotation about Y, and roll is rotation about X). The period, in milliseconds, specifies how often to send a transform. 100ms (10hz) is a good value.”

Map to Odom

If we want, for example, the map and odometry frame to be equivalent, here is the code you would write inside a ROS launch file:

<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 map odom 30" />

map -> odom transform tells us the position and orientation of the starting point of the robot (i.e. odom coordinate frame, which is the child) inside the map’s coordinate frame (i.e. the parent).

In our example above, we assume that the odom frame does not move relative to the map frame over time.

Odom to Base Footprint

odom -> base_footprint transform is not static because the robot moves around the world. The base_footprint coordinate frame will constantly change as the wheels turn. This non-static transform is often provided in packages like the robot_pose_ekf package.

Base Footprint to Base Link

base_footprint -> base_link is a static transform because both coordinate frames are fixed to each other. The robot moves, and the coordinate frame of its “footprint” will move.

<node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0.09 0 0 0 base_footprint base_link 30" />

In this case above, we assume that the origin of the base_link coordinate frame (i.e. center of the robot) is located 0.09 meters above its footprint.

Base Link to Laser

base_link -> laser transform gives us the position and orientation of the laser inside the base_link’s coordinate frame. This transform is static.

<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.06 0 0.08 0 0 0 base_link laser 30" />

We assume that the laser is located 0.06 meters forward of the center of the robot and 0.08 meters above the center of the robot.

Base Link to IMU

base_link -> imu transform gives us the position and orientation of the IMU (inertial measurement unit…commonly a BNO055 sensor) inside the base_link’s coordinate frame. This transform is static.

<node pkg="tf" type="static_transform_publisher" name="imu_broadcaster" args="0 0.06 0.02 0 0 0 base_link imu 30" />

That’s it for the basics of coordinate frames and static transform publishers in ROS. Keep building!

How to Create a Map for ROS From a Floor Plan or Blueprint

In this tutorial, I will show you how to create a map for RViz (3D visualizer program for ROS) from a floor plan or blueprint. Creating a map for RViz is an important step for enabling a robot to navigate autonomously in an environment. 

We often use a robot’s LIDAR to build a map. That is fine and good, but you can often build a better, more accurate map if you create one from a floor plan or blueprint you already have on hand.

Real-World Applications

This project has a number of real-world applications: 

  • Indoor Delivery Robots
  • Room Service Robots
  • Robot Vacuums
  • Order Fulfillment
  • Factories

Prerequisites

Directions

I will largely follow this YouTube video here, which explains the process step-by-step.

The first thing you need to do is to grab a floor plan or blueprint. Make sure it is in .png format.

Save the image to the directory where you want to eventually load your map.

I will create a new folder inside the catkin_ws called maps.

cd ~/catkin_ws
mkdir maps
cd maps

Install the map server.

sudo apt-get install ros-melodic-map-server

Edit the image file as you wish using a program like Paint.net.

mii_floor_plan_1

Convert the image to binary format using OpenCV. Here is the code for that. I named the program convert_to_binary.py.

import cv2 # Import OpenCV
  
# read the image file
img = cv2.imread('mii_floor_plan_4.png')
  
ret, bw_img = cv2.threshold(img, 220, 255, cv2.THRESH_BINARY)
  
# converting to its binary form
bw = cv2.threshold(img, 240, 255, cv2.THRESH_BINARY)
 
# Display and save image 
cv2.imshow("Binary", bw_img)
cv2.imwrite("mii_floor_plan_4_binary.png", bw_img)
cv2.waitKey(0)
cv2.destroyAllWindows()

The name of my image is mii_floor_plan_4_binary.png. Here is the image.

mii_floor_plan_4_binary

Now, let’s create our .pgm and .yaml map files. ROS needs both of these file formats to display the map on RViz. Write the code below inside the same directory as your .png image file.

gedit MakeROSMap.py
import numpy as np
import cv2
import math
import os.path

#
#  This is a start for the map program
#
prompt = '> '

print("What is the name of your floor plan you want to convert to a ROS map:") 
file_name = input(prompt)
print("You will need to choose the x coordinates horizontal with respect to each other")
print("Double Click the first x point to scale")
#
# Read in the image
#
image = cv2.imread(file_name)
#
# Some variables
#
ix,iy = -1,-1
x1 = [0,0,0,0]
y1 = [0,0,0,0]
font = cv2.FONT_HERSHEY_SIMPLEX
#
# mouse callback function
# This allows me to point and 
# it prompts me from the command line
#
def draw_point(event,x,y,flags,param):
  global ix,iy,x1,y1n,sx,sy
  if event == cv2.EVENT_LBUTTONDBLCLK:
    ix,iy = x,y
    print(ix,iy)
#
# Draws the point with lines around it so you can see it
#
    image[iy,ix]=(0,0,255)
    cv2.line(image,(ix+2,iy),(ix+10,iy),(0,0,255),1)
    cv2.line(image,(ix-2,iy),(ix-10,iy),(0,0,255),1)
    cv2.line(image,(ix,iy+2),(ix,iy+10),(0,0,255),1)
    cv2.line(image,(ix,iy-2),(ix,iy-10),(0,0,255),1)
#
# This is for the 4 mouse clicks and the x and y lengths
#
    if x1[0] == 0:
      x1[0]=ix
      y1[0]=iy
      print('Double click a second x point')   
    elif (x1[0] != 0 and x1[1] == 0):
      x1[1]=ix
      y1[1]=iy
      prompt = '> '
      print("What is the x distance in meters between the 2 points?") 
      deltax = float(input(prompt))
      dx = math.sqrt((x1[1]-x1[0])**2 + (y1[1]-y1[0])**2)*.05
      sx = deltax / dx
      print("You will need to choose the y coordinates vertical with respect to each other")
      print('Double Click a y point')
    elif (x1[1] != 0 and x1[2] == 0):
      x1[2]=ix
      y1[2]=iy
      print('Double click a second y point')
    else:
      prompt = '> '
      print("What is the y distance in meters between the 2 points?") 
      deltay = float(input(prompt))
      x1[3]=ix
      y1[3]=iy    
      dy = math.sqrt((x1[3]-x1[2])**2 + (y1[3]-y1[2])**2)*.05
      sy = deltay/dy 
      print(sx, sy)
      res = cv2.resize(image, None, fx=sx, fy=sy, interpolation = cv2.INTER_CUBIC)
      # Convert to grey
      res = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
      cv2.imwrite("KEC_BuildingCorrected.pgm", res );
      cv2.imshow("Image2", res)
      #for i in range(0,res.shape[1],20):
        #for j in range(0,res.shape[0],20):
          #res[j][i][0] = 0
          #res[j][i][1] = 0
          #res[j][i][2] = 0
      #cv2.imwrite("KEC_BuildingCorrectedDots.pgm",res)
	    # Show the image in a new window
	    #  Open a file
      prompt = '> '
      print("What is the name of the new map?")
      mapName = input(prompt)

      prompt = '> '
      print("Where is the desired location of the map and yaml file?") 
      print("NOTE: if this program is not run on the TurtleBot, Please input the file location of where the map should be saved on TurtleBot. The file will be saved at that location on this computer. Please then tranfer the files to TurtleBot.")
      mapLocation = input(prompt)
      completeFileNameMap = os.path.join(mapLocation, mapName +".pgm")
      completeFileNameYaml = os.path.join(mapLocation, mapName +".yaml")
      yaml = open(completeFileNameYaml, "w")
      cv2.imwrite(completeFileNameMap, res );
	    #
	    # Write some information into the file
	    #
      yaml.write("image: " + mapLocation + "/" + mapName + ".pgm\n")
      yaml.write("resolution: 0.050000\n")
      yaml.write("origin: [" + str(-1) + "," +  str(-1) + ", 0.000000]\n")
      yaml.write("negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196")
      yaml.close()
      exit()

cv2.namedWindow('image', cv2.WINDOW_NORMAL)
cv2.setMouseCallback('image',draw_point)
#
#  Waiting for a Esc hit to quit and close everything
#
while(1):
  cv2.imshow('image',image)
  k = cv2.waitKey(20) & 0xFF
  if k == 27:
    break
  elif k == ord('a'):
    print('Done')
cv2.destroyAllWindows()

To run the code, type:

python MakeROSMap.py

If that doesn’t work on your system, you might need to type:

python3 MakeROSMap.py

When the program asks you for the desired location, press Enter.

2021-05-29-14.21.44

My two output files are floorplan4.pgm and floorplan4.yaml.

Open the yaml file and put the full path for the pgm image in the image tag.

gedit floorplan4.yaml
image: /home/focalfossa/catkin_ws/maps/floorplan4.pgm
resolution: 0.050000
origin: [-1,-1, 0.000000]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

To find out the full path, you can go to the File Explorer in Linux and right-click on the image. Then go to Properties.

Save the file and close it.

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

cd ~/catkin_ws/maps
roscore

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

rosrun map_server map_server floorplan4.yaml
2021-05-29-14.28.46

Open rviz in another terminal.

rviz

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

Click OK.

Under Topic under the Map section, select /map.

You should see the saved map on your screen.

2021-05-30-21.31.22

That’s it. Keep building!

Sensor Fusion Using the ROS Robot Pose EKF Package

In this tutorial, we will learn how to set up an extended Kalman filter to fuse wheel encoder odometry information and IMU sensor information to create a better estimate of where a robot is located in the environment (i.e. localization).

sensor_fusion_using_ros-1

We will fuse odometry data (based on wheel encoder tick counts) with data from an IMU sensor (i.e. “sensor fusion”) to generate improved odometry data so that we can get regular estimates of the robot’s position and orientation as it moves about its environment. Accurate information is important for enabling a robot to navigate properly and build good maps.

An extended Kalman filter is the work horse behind all this. It provides a more robust estimate of the robot’s pose than using wheel encoders or IMU alone. The way to do this using ROS is to use the robot_pose_ekf package.

Real-World Applications

This project has a number of real-world applications: 

  • Indoor Delivery Robots
  • Room Service Robots
  • Mapping of Underground Mines, Caves, and Hard-to-Reach Environments
  • Robot Vacuums
  • Order Fulfillment
  • Factories

Prerequisites

Install the robot_pose_ekf Package

Let’s begin by installing the robot_pose_ekf package. Open a new terminal window, and type:

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

We are using ROS Melodic. If you are using ROS Noetic, you will need to substitute in ‘noetic’ for ‘melodic’.

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:

Create the Launch File

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 /odom_data_quat topic. The publisher for this topic is the node we created in this post.

The data for /imu_data  will come from the /imu/data topic. The publisher for this topic is the node we created in this post. 

In the launch file, we need to remap the data coming from the /odom_data_quat and /imu/data topics since the robot_pose_ekf node needs the topic names to be /odom and /imu_data, respectively.

Here is my full launch file. Don’t worry about trying to understand the static transform publishers at the top. I’ll cover that in a later post:

<launch>

  <!-- Transformation Configuration ... Setting Up the Relationships Between Coordinate Frames --> 
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.06 0 0.08 0 0 0 base_link laser 30" />
  <node pkg="tf" type="static_transform_publisher" name="imu_broadcaster" args="0 0.06 0.02 0 0 0 base_link imu 30" />
  <node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0.09 0 0 0 base_footprint base_link 30" />
  <!-- odom to base_footprint transform will be provided by the robot_pose_ekf node -->

  <!-- Wheel Encoder Tick Publisher and Base Controller Using Arduino -->  
  <!-- motor_controller_diff_drive_2.ino is the Arduino sketch -->
  <!-- Subscribe: /cmd_vel -->
  <!-- Publish: /right_ticks, /left_ticks -->
  <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
    <param name="port" value="/dev/ttyACM0"/>
    <param name="baud" value="115200"/>
  </node>

  <!-- Wheel Odometry Publisher -->
  <!-- Subscribe: /right_ticks, /left_ticks, /initial_2d -->
  <!-- Publish: /odom_data_euler, /odom_data_quat -->
  <node pkg="localization_data_pub" type="ekf_odom_pub" name="ekf_odom_pub">
  </node> 
	
  <!-- IMU Data Publisher Using the BNO055 IMU Sensor -->
  <!-- Publish: /imu/data -->
  <node ns="imu" name="imu_node" pkg="imu_bno055" type="bno055_i2c_node" respawn="true" respawn_delay="2">
    <param name="device" type="string" value="/dev/i2c-1"/>
    <param name="address" type="int" value="40"/> <!-- 0x28 == 40 is the default for BNO055 -->
    <param name="frame_id" type="string" value="imu"/>
  </node>
	
  <!-- Extended Kalman Filter from robot_pose_ekf Node-->
  <!-- Subscribe: /odom, /imu_data, /vo -->
  <!-- Publish: /robot_pose_ekf/odom_combined -->
  <remap from="odom" to="odom_data_quat" />
  <remap from="imu_data" to="imu/data" />
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/>
    <param name="base_footprint_frame" value="base_footprint"/>
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="gps_used" value="false"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>
	
  <!-- Initial Pose and Goal Publisher -->
  <!-- Publish: /initialpose, /move_base_simple/goal -->
  <node pkg="rviz" type="rviz" name="rviz">
  </node> 

  <!-- Subscribe: /initialpose, /move_base_simple/goal -->
  <!-- Publish: /initial_2d, /goal_2d --> 
  <node pkg="localization_data_pub" type="rviz_click_to_2d" name="rviz_click_to_2d">
  </node>   

</launch>

You can check out this post to learn how to run ROS launch files. 

That’s it. Keep building!