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



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


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

import cv2 # Import OpenCV
# read the image file
img = cv2.imread('mii_floor_plan_3.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_3_binary.png", bw_img)

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


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.

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]
# 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
# Draws the point with lines around it so you can see it
# This is for the 4 mouse clicks and the x and y lengths
    if x1[0] == 0:
      print('Double click a second x point')   
    elif (x1[0] != 0 and x1[1] == 0):
      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):
      print('Double click a second y point')
      prompt = '> '
      print("What is the y distance in meters between the 2 points?") 
      deltay = float(input(prompt))
      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
	    # 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")

cv2.namedWindow('image', cv2.WINDOW_NORMAL)
#  Waiting for a Esc hit to quit and close everything
  k = cv2.waitKey(20) & 0xFF
  if k == 27:
  elif k == ord('a'):

To run the code, type:


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


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


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

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

rosrun map_server map_server floorplan4.yaml

Open rviz in another terminal.


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.


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).


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


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.


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:


  <!-- 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="" name="serial_node">
    <param name="port" value="/dev/ttyACM0"/>
    <param name="baud" value="115200"/>

  <!-- 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">
  <!-- 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"/>
  <!-- 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"/>
  <!-- Initial Pose and Goal Publisher -->
  <!-- Publish: /initialpose, /move_base_simple/goal -->
  <node pkg="rviz" type="rviz" name="rviz">

  <!-- 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">


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

That’s it. Keep building!

How to Publish Wheel Odometry Information Over ROS

In this tutorial, we will learn how to publish wheel odometry information over ROS. We will assume a two-wheeled differential drive robot.

In robotics, odometry is about using data from sensors (e.g. wheel encoders) to estimate the change in the robot’s position and orientation over time relative to some world-fixed point (e.g. x=0,y=0,z=0). We use trigonometry at each timestep along with the data from the wheel encoders to generate estimates of where the robot is in the world and how it is oriented.

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


Getting Started

Let’s create an odometry publisher that is based on wheel encoder data (no IMU inputs). This node will subscribe to the following topics (ROS message types are in parentheses):

The publisher will publish odometry data to the following topics:

Open a new terminal window.

Move to the src folder of the localization package.

cd ~/catkin_ws/src/jetson_nano_bot/localization_data_pub/src

Open a new C++ file called ekf_odom_pub.cpp.

gedit ekf_odom_pub.cpp

Write the following code inside the file, then save and close it. I won’t go into the code in detail, but I added a lot of comments so you can understand what is going on at each step. All you need to change at the values of the variables to fit your robot. Leave everything else as-is.

 * Automatic Addison
 * Date: May 20, 2021
 * ROS Version: ROS 1 - Melodic
 * Website:
 * Publishes odometry information for use with robot_pose_ekf package.
 *   This odometry information is based on wheel encoder tick counts.
 * Subscribe: ROS node that subscribes to the following topics:
 *  right_ticks : Tick counts from the right motor encoder (std_msgs/Int16)
 *  left_ticks : Tick counts from the left motor encoder  (std_msgs/Int16)
 *  initial_2d : The initial position and orientation of the robot.
 *               (geometry_msgs/PoseStamped)
 * Publish: This node will publish to the following topics:
 *  odom_data_euler : Position and velocity estimate. The orientation.z 
 *                    variable is an Euler angle representing the yaw angle.
 *                    (nav_msgs/Odometry)
 *  odom_data_quat : Position and velocity estimate. The orientation is 
 *                   in quaternion format.
 *                   (nav_msgs/Odometry)
 * Modified from Practical Robotics in C++ book (ISBN-10 : 9389423465)
 *   by Lloyd Brombach

// Include various libraries
#include "ros/ros.h"
#include "std_msgs/Int16.h"
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <cmath>

// Create odometry data publishers
ros::Publisher odom_data_pub;
ros::Publisher odom_data_pub_quat;
nav_msgs::Odometry odomNew;
nav_msgs::Odometry odomOld;

// Initial pose
const double initialX = 0.0;
const double initialY = 0.0;
const double initialTheta = 0.00000000001;
const double PI = 3.141592;

// Robot physical constants
const double TICKS_PER_REVOLUTION = 620; // For reference purposes.
const double WHEEL_RADIUS = 0.033; // Wheel radius in meters
const double WHEEL_BASE = 0.17; // Center of left tire to center of right tire
const double TICKS_PER_METER = 3100; // Original was 2800

// Distance both wheels have traveled
double distanceLeft = 0;
double distanceRight = 0;

// Flag to see if initial pose has been received
bool initialPoseRecieved = false;

using namespace std;

// Get initial_2d message from either Rviz clicks or a manual pose publisher
void set_initial_2d(const geometry_msgs::PoseStamped &rvizClick) {

  odomOld.pose.pose.position.x = rvizClick.pose.position.x;
  odomOld.pose.pose.position.y = rvizClick.pose.position.y;
  odomOld.pose.pose.orientation.z = rvizClick.pose.orientation.z;
  initialPoseRecieved = true;

// Calculate the distance the left wheel has traveled since the last cycle
void Calc_Left(const std_msgs::Int16& leftCount) {

  static int lastCountL = 0;
  if( != 0 && lastCountL != 0) {
    int leftTicks = ( - lastCountL);

    if (leftTicks > 10000) {
      leftTicks = 0 - (65535 - leftTicks);
    else if (leftTicks < -10000) {
      leftTicks = 65535-leftTicks;
    distanceLeft = leftTicks/TICKS_PER_METER;
  lastCountL =;

// Calculate the distance the right wheel has traveled since the last cycle
void Calc_Right(const std_msgs::Int16& rightCount) {
  static int lastCountR = 0;
  if( != 0 && lastCountR != 0) {

    int rightTicks = - lastCountR;
    if (distanceRight > 10000) {
      distanceRight = (0 - (65535 - distanceRight))/TICKS_PER_METER;
    else if (rightTicks < -10000) {
      rightTicks = 65535 - rightTicks;
    distanceRight = rightTicks/TICKS_PER_METER;
  lastCountR =;

// Publish a nav_msgs::Odometry message in quaternion format
void publish_quat() {

  tf2::Quaternion q;
  q.setRPY(0, 0, odomNew.pose.pose.orientation.z);

  nav_msgs::Odometry quatOdom;
  quatOdom.header.stamp = odomNew.header.stamp;
  quatOdom.header.frame_id = "odom";
  quatOdom.child_frame_id = "base_link";
  quatOdom.pose.pose.position.x = odomNew.pose.pose.position.x;
  quatOdom.pose.pose.position.y = odomNew.pose.pose.position.y;
  quatOdom.pose.pose.position.z = odomNew.pose.pose.position.z;
  quatOdom.pose.pose.orientation.x = q.x();
  quatOdom.pose.pose.orientation.y = q.y();
  quatOdom.pose.pose.orientation.z = q.z();
  quatOdom.pose.pose.orientation.w = q.w();
  quatOdom.twist.twist.linear.x = odomNew.twist.twist.linear.x;
  quatOdom.twist.twist.linear.y = odomNew.twist.twist.linear.y;
  quatOdom.twist.twist.linear.z = odomNew.twist.twist.linear.z;
  quatOdom.twist.twist.angular.x = odomNew.twist.twist.angular.x;
  quatOdom.twist.twist.angular.y = odomNew.twist.twist.angular.y;
  quatOdom.twist.twist.angular.z = odomNew.twist.twist.angular.z;

  for(int i = 0; i<36; i++) {
    if(i == 0 || i == 7 || i == 14) {
      quatOdom.pose.covariance[i] = .01;
     else if (i == 21 || i == 28 || i== 35) {
       quatOdom.pose.covariance[i] += 0.1;
     else {
       quatOdom.pose.covariance[i] = 0;


// Update odometry information
void update_odom() {

  // Calculate the average distance
  double cycleDistance = (distanceRight + distanceLeft) / 2;
  // Calculate the number of radians the robot has turned since the last cycle
  double cycleAngle = asin((distanceRight-distanceLeft)/WHEEL_BASE);

  // Average angle during the last cycle
  double avgAngle = cycleAngle/2 + odomOld.pose.pose.orientation.z;
  if (avgAngle > PI) {
    avgAngle -= 2*PI;
  else if (avgAngle < -PI) {
    avgAngle += 2*PI;

  // Calculate the new pose (x, y, and theta)
  odomNew.pose.pose.position.x = odomOld.pose.pose.position.x + cos(avgAngle)*cycleDistance;
  odomNew.pose.pose.position.y = odomOld.pose.pose.position.y + sin(avgAngle)*cycleDistance;
  odomNew.pose.pose.orientation.z = cycleAngle + odomOld.pose.pose.orientation.z;

  // Prevent lockup from a single bad cycle
  if (isnan(odomNew.pose.pose.position.x) || isnan(odomNew.pose.pose.position.y)
     || isnan(odomNew.pose.pose.position.z)) {
    odomNew.pose.pose.position.x = odomOld.pose.pose.position.x;
    odomNew.pose.pose.position.y = odomOld.pose.pose.position.y;
    odomNew.pose.pose.orientation.z = odomOld.pose.pose.orientation.z;

  // Make sure theta stays in the correct range
  if (odomNew.pose.pose.orientation.z > PI) {
    odomNew.pose.pose.orientation.z -= 2 * PI;
  else if (odomNew.pose.pose.orientation.z < -PI) {
    odomNew.pose.pose.orientation.z += 2 * PI;

  // Compute the velocity
  odomNew.header.stamp = ros::Time::now();
  odomNew.twist.twist.linear.x = cycleDistance/(odomNew.header.stamp.toSec() - odomOld.header.stamp.toSec());
  odomNew.twist.twist.angular.z = cycleAngle/(odomNew.header.stamp.toSec() - odomOld.header.stamp.toSec());

  // Save the pose data for the next cycle
  odomOld.pose.pose.position.x = odomNew.pose.pose.position.x;
  odomOld.pose.pose.position.y = odomNew.pose.pose.position.y;
  odomOld.pose.pose.orientation.z = odomNew.pose.pose.orientation.z;
  odomOld.header.stamp = odomNew.header.stamp;

  // Publish the odometry message

int main(int argc, char **argv) {
  // Set the data fields of the odometry message
  odomNew.header.frame_id = "odom";
  odomNew.pose.pose.position.z = 0;
  odomNew.pose.pose.orientation.x = 0;
  odomNew.pose.pose.orientation.y = 0;
  odomNew.twist.twist.linear.x = 0;
  odomNew.twist.twist.linear.y = 0;
  odomNew.twist.twist.linear.z = 0;
  odomNew.twist.twist.angular.x = 0;
  odomNew.twist.twist.angular.y = 0;
  odomNew.twist.twist.angular.z = 0;
  odomOld.pose.pose.position.x = initialX;
  odomOld.pose.pose.position.y = initialY;
  odomOld.pose.pose.orientation.z = initialTheta;

  // Launch ROS and create a node
  ros::init(argc, argv, "ekf_odom_pub");
  ros::NodeHandle node;

  // Subscribe to ROS topics
  ros::Subscriber subForRightCounts = node.subscribe("right_ticks", 100, Calc_Right, ros::TransportHints().tcpNoDelay());
  ros::Subscriber subForLeftCounts = node.subscribe("left_ticks", 100, Calc_Left, ros::TransportHints().tcpNoDelay());
  ros::Subscriber subInitialPose = node.subscribe("initial_2d", 1, set_initial_2d);

  // Publisher of simple odom message where orientation.z is an euler angle
  odom_data_pub = node.advertise<nav_msgs::Odometry>("odom_data_euler", 100);

  // Publisher of full odom message where orientation is quaternion
  odom_data_pub_quat = node.advertise<nav_msgs::Odometry>("odom_data_quat", 100);

  ros::Rate loop_rate(30); 
  while(ros::ok()) {
    if(initialPoseRecieved) {

  return 0;

Now we need to add the C++ program we just wrote to the CMakeLists.txt file.

cd ~/catkin_ws/src/jetson_nano_bot/localization_data_pub/
gedit CMakeLists.txt

Go to the bottom of the file.

Add the following lines.

add_executable(ekf_odom_pub src/ekf_odom_pub.cpp)
target_link_libraries(ekf_odom_pub ${catkin_LIBRARIES})

Save the file, and close it.

Go to the root of the workspace.

cd ~/catkin_ws

Compile the code.

catkin_make --only-pkg-with-deps localization_data_pub 

Now let’s run the ROS node to test it.

Open a new terminal window.

Start ROS.


Open another terminal window, and launch the node.

rosrun localization_data_pub ekf_odom_pub

Start the tick count publisher.

rosrun rosserial_python _port:=/dev/ttyACM0 _baud:=115200

Open another terminal window, and launch the initial pose and goal publisher.

rosrun localization_data_pub rviz_click_to_2d

Set the initial pose of the robot using the button at the top of RViz. 

Set the goal destination using the button at the top of RViz.

If everything is working properly, you should see output when you type the following in a new terminal window.

rostopic echo /odom_data_quat

Add the Wheel Odometry Publisher to a Launch File

To add the wheel odometry publisher above to a ROS launch file, you will add the following lines: 

  <!-- 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">

That’s it! Keep building!