In this tutorial, we will learn how to use an NVIDIA Jetson Nano to read data from an MPU6050 IMU (Inertial measurement unit) sensor. We will also learn how to connect the MPU6050’s data to ROS, the most popular framework in the world for writing robot software. Here is what the final output will look like:

Real-World Applications

IMUs like the MPU6050 enable us to measure the acceleration and orientation of a robotic system. IMUs are common in both wheeled robots and aerial robots.

Prerequisites

• You have already set up your NVIDIA Jetson Nano (4GB, B01) with ROS.

You Will Need

In addition to the parts listed in the article I linked to in the Prerequisites, you will need the following components (#ad).

• MPU6050 (to make sure the electrical pins are stuck to the board, you can solder them like I did in this video here with the BNO055 board)
• Assorted breadboard jumper wires
• 400-point solderless breadboard (Optional)

Once you finish this project, you can mount the IMU on anything that moves (drone, robot car, etc.) so you can measure the system’s acceleration and orientation.

Disclosure (#ad): As an Amazon Associate I earn from qualifying purchases.

Set Up the Hardware

Here is the pinout diagram for the NVIDIA Jetson Nano B01.

Make the following connections:

• Connect VCC of the MPU6050 to pin 17 (3.3V) of the Jetson Nano.
• Connect GND of the MPU6050 to pin 25 (GND) of the Jetson Nano.
• Connect SCL of the MPU6050 to pin 5 (SCL) of the Jetson Nano.
• Connect SDA of the MPU6050 to pin 3 (SDA) of the Jetson Nano.

Set Up the Communication Protocol

Our MPU6050 will use the I2C serial communication protocol. What this means is that data will be transferred from the IMU to the Jetson Nano one bit at a time.

Turn on your Jetson Nano.

Open a terminal window.

Type the following command to verify that you can see the MPU6050.

sudo i2cdetect -r -y 1

You can see that the address of the MPU6050 is 68.

Now let’s go through the different ways we can connect the MPU6050 to the Jetson Nano. We’ll go through one option at a time.

Write the Code

Option 1: Use the Adafruit Circuit Python Library

Install the adafruit-blinka library.

sudo apt-get update

pip3 install adafruit-blinka

It will take some time to download everything, so be patient. It might even look like it is frozen during the downloading process, but don’t worry, it is downloading.

Test that everything is installed correctly.

Make a new directory named mpu6050_test.

mkdir mpu6050_test

Move to that directory.

cd mpu6050_test

Create a Python program called blinkatest.py.

gedit blinkatest.py

Write the following code.

import board
import busio

print("Hello blinka!")

# Try to create an I2C device
i2c = busio.I2C(board.SCL, board.SDA)
print("I2C 1 ok!")

print("done!")

Press save, and close it.

Run the code:

python3 blinkatest.py

Now let’s work with the Adafruit MPU6050 library.

Install the MPU6050 library.

pip3 install adafruit-circuitpython-mpu6050

Create a new program called mpu6050_simpletest.py.

gedit mpu6050_simpletest.py

Write the following code inside it. Credit to Adafruit for this code.

Save the file and close it.

Run the code.

python3 mpu6050_simpletest.py

Press CTRL+C when you’re done running the program.

Option 2: Using smbus

Here is another approach to reading data from the MPU6050. Credit to ElectronicWings for this method.

Install the smbus library.

sudo apt-get install python3-smbus

Create a program called mpu6050_simpletest2.py.

gedit mpu6050_simpletest2.py

Write the following code inside it.

'''
        Read Gyro and Accelerometer by Interfacing Raspberry Pi with MPU6050 using Python
	http://www.electronicwings.com
'''
import smbus			#import SMBus module of I2C
from time import sleep          #import

#some MPU6050 Registers and their Address
PWR_MGMT_1   = 0x6B
SMPLRT_DIV   = 0x19
CONFIG       = 0x1A
GYRO_CONFIG  = 0x1B
INT_ENABLE   = 0x38
ACCEL_XOUT_H = 0x3B
ACCEL_YOUT_H = 0x3D
ACCEL_ZOUT_H = 0x3F
GYRO_XOUT_H  = 0x43
GYRO_YOUT_H  = 0x45
GYRO_ZOUT_H  = 0x47


def MPU_Init():
	#write to sample rate register
	bus.write_byte_data(Device_Address, SMPLRT_DIV, 7)
	
	#Write to power management register
	bus.write_byte_data(Device_Address, PWR_MGMT_1, 1)
	
	#Write to Configuration register
	bus.write_byte_data(Device_Address, CONFIG, 0)
	
	#Write to Gyro configuration register
	bus.write_byte_data(Device_Address, GYRO_CONFIG, 24)
	
	#Write to interrupt enable register
	bus.write_byte_data(Device_Address, INT_ENABLE, 1)

def read_raw_data(addr):
	#Accelero and Gyro value are 16-bit
        high = bus.read_byte_data(Device_Address, addr)
        low = bus.read_byte_data(Device_Address, addr+1)
    
        #concatenate higher and lower value
        value = ((high << 8) | low)
        
        #to get signed value from mpu6050
        if(value > 32768):
                value = value - 65536
        return value


bus = smbus.SMBus(1) 	# or bus = smbus.SMBus(0) for older version boards
Device_Address = 0x68   # MPU6050 device address

MPU_Init()

print (" Reading Data of Gyroscope and Accelerometer")

while True:
	
	#Read Accelerometer raw value
	acc_x = read_raw_data(ACCEL_XOUT_H)
	acc_y = read_raw_data(ACCEL_YOUT_H)
	acc_z = read_raw_data(ACCEL_ZOUT_H)
	
	#Read Gyroscope raw value
	gyro_x = read_raw_data(GYRO_XOUT_H)
	gyro_y = read_raw_data(GYRO_YOUT_H)
	gyro_z = read_raw_data(GYRO_ZOUT_H)
	
	#Full scale range +/- 250 degree/C as per sensitivity scale factor
	Ax = acc_x/16384.0
	Ay = acc_y/16384.0
	Az = acc_z/16384.0
	
	Gx = gyro_x/131.0
	Gy = gyro_y/131.0
	Gz = gyro_z/131.0
	

	print ("Gx=%.2f" %Gx, u'\u00b0'+ "/s", "\tGy=%.2f" %Gy, u'\u00b0'+ "/s", "\tGz=%.2f" %Gz, u'\u00b0'+ "/s", "\tAx=%.2f g" %Ax, "\tAy=%.2f g" %Ay, "\tAz=%.2f g" %Az) 	
	sleep(1)

Save the file and close it.

Run the code.

python3 mpu6050_simpletest2.py

Here is the output:

Option 3: Using smbus again

Here is our third and final approach to reading data from the MPU6050. 

Create a program called mpu6050_simpletest3.py.

gedit mpu6050_simpletest3.py

Write the following code inside it.

"""This program handles the communication over I2C
between a Jetson Nano and a MPU-6050 Gyroscope / Accelerometer combo.
Made by: Dennis/TW
Released under the MIT License
Copyright 2019
"""

import smbus
from time import sleep      

class mpu6050:

    # Global Variables
    GRAVITIY_MS2 = 9.80665
    address = None
    bus = smbus.SMBus(1)

    # Scale Modifiers
    ACCEL_SCALE_MODIFIER_2G = 16384.0
    ACCEL_SCALE_MODIFIER_4G = 8192.0
    ACCEL_SCALE_MODIFIER_8G = 4096.0
    ACCEL_SCALE_MODIFIER_16G = 2048.0

    GYRO_SCALE_MODIFIER_250DEG = 131.0
    GYRO_SCALE_MODIFIER_500DEG = 65.5
    GYRO_SCALE_MODIFIER_1000DEG = 32.8
    GYRO_SCALE_MODIFIER_2000DEG = 16.4

    # Pre-defined ranges
    ACCEL_RANGE_2G = 0x00
    ACCEL_RANGE_4G = 0x08
    ACCEL_RANGE_8G = 0x10
    ACCEL_RANGE_16G = 0x18

    GYRO_RANGE_250DEG = 0x00
    GYRO_RANGE_500DEG = 0x08
    GYRO_RANGE_1000DEG = 0x10
    GYRO_RANGE_2000DEG = 0x18

    # MPU-6050 Registers
    PWR_MGMT_1 = 0x6B
    PWR_MGMT_2 = 0x6C

    SELF_TEST_X = 0x0D
    SELF_TEST_Y = 0x0E
    SELF_TEST_Z = 0x0F
    SELF_TEST_A = 0x10

    ACCEL_XOUT0 = 0x3B
    ACCEL_XOUT1 = 0x3C
    ACCEL_YOUT0 = 0x3D
    ACCEL_YOUT1 = 0x3E
    ACCEL_ZOUT0 = 0x3F
    ACCEL_ZOUT1 = 0x40

    TEMP_OUT0 = 0x41
    TEMP_OUT1 = 0x42

    GYRO_XOUT0 = 0x43
    GYRO_XOUT1 = 0x44
    GYRO_YOUT0 = 0x45
    GYRO_YOUT1 = 0x46
    GYRO_ZOUT0 = 0x47
    GYRO_ZOUT1 = 0x48

    ACCEL_CONFIG = 0x1C
    GYRO_CONFIG = 0x1B

    def __init__(self, address):
        self.address = address

        # Wake up the MPU-6050 since it starts in sleep mode
        self.bus.write_byte_data(self.address, self.PWR_MGMT_1, 0x00)

    # I2C communication methods

    def read_i2c_word(self, register):
        """Read two i2c registers and combine them.

        register -- the first register to read from.
        Returns the combined read results.
        """
        # Read the data from the registers
        high = self.bus.read_byte_data(self.address, register)
        low = self.bus.read_byte_data(self.address, register + 1)

        value = (high << 8) + low

        if (value >= 0x8000):
            return -((65535 - value) + 1)
        else:
            return value

    # MPU-6050 Methods

    def get_temp(self):
        """Reads the temperature from the onboard temperature sensor of the MPU-6050.

        Returns the temperature in degrees Celcius.
        """
        # Get the raw data
        raw_temp = self.read_i2c_word(self.TEMP_OUT0)

        # Get the actual temperature using the formule given in the
        # MPU-6050 Register Map and Descriptions revision 4.2, page 30
        actual_temp = (raw_temp / 340) + 36.53

        # Return the temperature
        return actual_temp

    def set_accel_range(self, accel_range):
        """Sets the range of the accelerometer to range.

        accel_range -- the range to set the accelerometer to. Using a
        pre-defined range is advised.
        """
        # First change it to 0x00 to make sure we write the correct value later
        self.bus.write_byte_data(self.address, self.ACCEL_CONFIG, 0x00)

        # Write the new range to the ACCEL_CONFIG register
        self.bus.write_byte_data(self.address, self.ACCEL_CONFIG, accel_range)

    def read_accel_range(self, raw = False):
        """Reads the range the accelerometer is set to.

        If raw is True, it will return the raw value from the ACCEL_CONFIG
        register
        If raw is False, it will return an integer: -1, 2, 4, 8 or 16. When it
        returns -1 something went wrong.
        """
        # Get the raw value
        raw_data = self.bus.read_byte_data(self.address, self.ACCEL_CONFIG)

        if raw is True:
            return raw_data
        elif raw is False:
            if raw_data == self.ACCEL_RANGE_2G:
                return 2
            elif raw_data == self.ACCEL_RANGE_4G:
                return 4
            elif raw_data == self.ACCEL_RANGE_8G:
                return 8
            elif raw_data == self.ACCEL_RANGE_16G:
                return 16
            else:
                return -1

    def get_accel_data(self, g = False):
        """Gets and returns the X, Y and Z values from the accelerometer.

        If g is True, it will return the data in g
        If g is False, it will return the data in m/s^2
        Returns a dictionary with the measurement results.
        """
        # Read the data from the MPU-6050
        x = self.read_i2c_word(self.ACCEL_XOUT0)
        y = self.read_i2c_word(self.ACCEL_YOUT0)
        z = self.read_i2c_word(self.ACCEL_ZOUT0)

        accel_scale_modifier = None
        accel_range = self.read_accel_range(True)

        if accel_range == self.ACCEL_RANGE_2G:
            accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_2G
        elif accel_range == self.ACCEL_RANGE_4G:
            accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_4G
        elif accel_range == self.ACCEL_RANGE_8G:
            accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_8G
        elif accel_range == self.ACCEL_RANGE_16G:
            accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_16G
        else:
            print("Unkown range - accel_scale_modifier set to self.ACCEL_SCALE_MODIFIER_2G")
            accel_scale_modifier = self.ACCEL_SCALE_MODIFIER_2G

        x = x / accel_scale_modifier
        y = y / accel_scale_modifier
        z = z / accel_scale_modifier

        if g is True:
            return {'x': x, 'y': y, 'z': z}
        elif g is False:
            x = x * self.GRAVITIY_MS2
            y = y * self.GRAVITIY_MS2
            z = z * self.GRAVITIY_MS2
            return {'x': x, 'y': y, 'z': z}

    def set_gyro_range(self, gyro_range):
        """Sets the range of the gyroscope to range.

        gyro_range -- the range to set the gyroscope to. Using a pre-defined
        range is advised.
        """
        # First change it to 0x00 to make sure we write the correct value later
        self.bus.write_byte_data(self.address, self.GYRO_CONFIG, 0x00)

        # Write the new range to the ACCEL_CONFIG register
        self.bus.write_byte_data(self.address, self.GYRO_CONFIG, gyro_range)

    def read_gyro_range(self, raw = False):
        """Reads the range the gyroscope is set to.

        If raw is True, it will return the raw value from the GYRO_CONFIG
        register.
        If raw is False, it will return 250, 500, 1000, 2000 or -1. If the
        returned value is equal to -1 something went wrong.
        """
        # Get the raw value
        raw_data = self.bus.read_byte_data(self.address, self.GYRO_CONFIG)

        if raw is True:
            return raw_data
        elif raw is False:
            if raw_data == self.GYRO_RANGE_250DEG:
                return 250
            elif raw_data == self.GYRO_RANGE_500DEG:
                return 500
            elif raw_data == self.GYRO_RANGE_1000DEG:
                return 1000
            elif raw_data == self.GYRO_RANGE_2000DEG:
                return 2000
            else:
                return -1

    def get_gyro_data(self):
        """Gets and returns the X, Y and Z values from the gyroscope.

        Returns the read values in a dictionary.
        """
        # Read the raw data from the MPU-6050
        x = self.read_i2c_word(self.GYRO_XOUT0)
        y = self.read_i2c_word(self.GYRO_YOUT0)
        z = self.read_i2c_word(self.GYRO_ZOUT0)

        gyro_scale_modifier = None
        gyro_range = self.read_gyro_range(True)

        if gyro_range == self.GYRO_RANGE_250DEG:
            gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_250DEG
        elif gyro_range == self.GYRO_RANGE_500DEG:
            gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_500DEG
        elif gyro_range == self.GYRO_RANGE_1000DEG:
            gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_1000DEG
        elif gyro_range == self.GYRO_RANGE_2000DEG:
            gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_2000DEG
        else:
            print("Unkown range - gyro_scale_modifier set to self.GYRO_SCALE_MODIFIER_250DEG")
            gyro_scale_modifier = self.GYRO_SCALE_MODIFIER_250DEG

        x = x / gyro_scale_modifier
        y = y / gyro_scale_modifier
        z = z / gyro_scale_modifier

        return {'x': x, 'y': y, 'z': z}

    def get_all_data(self):
        """Reads and returns all the available data."""
        temp = get_temp()
        accel = get_accel_data()
        gyro = get_gyro_data()

        return [accel, gyro, temp]

if __name__ == "__main__":
    while(1):
        mpu = mpu6050(0x68)
        print("Temperature (C): ", mpu.get_temp())
        accel_data = mpu.get_accel_data()
        print("Acceleration x (m/s^2): ", accel_data['x'])
        print("Acceleration y (m/s^2): ", accel_data['y'])
        print("Acceleration z (m/s^2): ", accel_data['z'])
        gyro_data = mpu.get_gyro_data()
        print("Gyroscope x (deg/s): ", gyro_data['x'])
        print("Gyroscope y (deg/s): ", gyro_data['y'])
        print("Gyroscope z (deg/s): ", gyro_data['z'])
        sleep(1)

Save the file and close it.

Run the code.

python mpu6050_simpletest3.py

Here is the output:

Connect the MPU6050 IMU to ROS

Now let’s connect the MPU6050 to ROS. We want to publish the data we read from the MPU6050 to a ROS topic. Let’s take a look at two different ways to do this using pre-built ROS packages.

Option 1

Install and Build the Package

Go to your catkin workspace.

cd ~/catkin_ws/src

Clone the package. This package was created by roboticists at Oregon State University.

git clone https://github.com/OSUrobotics/mpu_6050_driver.git

cd mpu_6050_driver/scripts

gedit tf_broadcaster_imu.py 

Write this code.

Save the file, and close it.

Open the imu_node.py file.

gedit imu_node.py

Go down to lines 93 and 94, and add a queue_size of 10. Here is how the code should look:

Save the file and close it.

Change permissions of both files.

chmod +x imu_node.py

chmod +x tf_broadcaster_imu.py

Move to the root.

cd ~/catkin_ws/

Build the package.

catkin_make

Open a new terminal window, and print the path to your new ROS package named mpu_6050_driver.

rospack find mpu_6050_driver

To see if our package depends on other packages, type these commands. Ignore any error messages you see in the terminal:

rosdep update

rosdep check mpu_6050_driver

You should see a message that says “All system dependencies have been satisfied.”

Run the Node

Now we want to run the node that is inside the mpu_6050_driver package.

Open a new terminal window, and launch ROS.

cd ~/catkin_ws/

roscore

Open another terminal window, and run the IMU node.

rosrun mpu_6050_driver imu_node.py

Open another terminal window, and type the following command:

rosrun mpu_6050_driver tf_broadcaster_imu.py

Open Rviz.

rosrun rviz rviz

Change the Fixed Frame parameter under Global Options to imu_link.

Click the Add button in the bottom-left of the Displays menu.

Add TF.

Also add Imu if you have that option. Under the Imu dropdown in the Displays, change the Topic to /imu/data.

Let’s see the active ROS topics. Open up a new terminal and type:

rostopic list

Open another terminal window, and visualize the imu data.

rostopic echo /imu/data

Press CTRL+C on all windows when you’re done.

Option 2

Install and Build the Package

For this option, we will use the IMU Complementary Filter package. Type the following commands to install this package.

cd ~/catkin_ws/src

sudo apt-get install ros-melodic-imu-tools

Install the MPU 6050 driver package created by Robotics and ROS Learning. He gives a great walkthrough about this package in this video on YouTube. Type this single command below.

git clone https://github.com/bandasaikrishna/orientations_from_IMU_MPU_6050.git

Using the file explorer on your Jetson Nano, copy all of the contents of that folder you just imported from GitHub and paste them into your mpu_6050_driver package from the previous section. When asked if you want to overwrite, yes you want to overwrite all duplicated folders.

Go to the scripts folder.

cd ~/catkin_ws/src/mpu_6050_driver/scripts

Change permissions on the imu_node.py file and tf_broadcaster_imu.py nodes.

chmod +x imu_node.py

chmod +x tf_broadcaster_imu.py

Build the package.

cd ~/catkin_ws/

catkin_make

Check dependencies. Open a new terminal window, and type:

rosdep check mpu_6050_driver

You should see a message that says “All system dependencies have been satisfied.”

Launch It

Now we want to launch the package.

Open a new terminal window, and type the following command:

roslaunch mpu_6050_driver imu_demo.launch

Let’s see the active ROS topics. Open up a new terminal and type:

rostopic list

That’s it! If for some reason, that GitHub code is no longer available, you can download the code here from my files.

Keep building!

In this tutorial, I will show you how to build a map using LIDAR, ROS 1 (Melodic), Hector SLAM, and NVIDIA Jetson Nano. We will go through the entire process, step-by-step. You can combine what you will learn in this tutorial with an obstacle avoiding robot to build a map of any indoor environment. Below is a small robot I built that wanders around the room while generating a map.

Go through this tutorial slowly so that you set up everything correctly. To go fast, go slow. There is no hurry. Follow along, click-by-click, part-by-part as we develop a complete LIDAR-based SLAM system from scratch.

Real-World Applications

This project has a number of real-world applications: 

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

Prerequisites

• You have already set up your NVIDIA Jetson Nano (4GB, B01) with ROS.

You Will Need

In addition to the parts listed in the article I linked to in the Prerequisites, you will need the following components (#ad).

• INIU Power Bank, LED Display 10000mAh Portable Charger
• Slamtec RPLIDAR A1M8 360 Degree 2D Laser Range Lidar Sensor 12 Meters Scanning Radius
• USB to microUSB Cable
• Acrylic Case for Jetson Nano B01 With a Dedicated Cooling Fan (Optional)

Once you finish this project, you can mount your Jetson Nano and LIDAR on anything that moves (drone, robot car, etc.) so you can map an environment.

Disclosure (#ad): As an Amazon Associate I earn from qualifying purchases.

Test the Power Bank

The portable power bank that I purchased can be used to power your Jetson Nano. It outputs 5V/3A. Having a portable power bank enables you to move the Jetson Nano and LIDAR around a room freely so you can build a map.

First, take the jumper off of pin J48 so your Jetson Nano knows we are going to plug in the portable battery into the 5V/2A USB Micro B connection (J28) rather than the 5V/4A barrel power jack (J25).

Turn on your Jetson Nano. If your Jetson Nano turns on successfully, the power bank is working.

Shutdown your Jetson Nano.

sudo shutdown -h now

Connect Your RPLIDAR

Connect your RPLIDAR to the Jetson Nano using the USB to microUSB cable.

Turn on your Jetson Nano.

Open a terminal window, and check the permissions. You can copy and paste this command below into the terminal. Note that -l is a lowercase L.

ls -l /dev | grep ttyUSB

Here is the output you should see:

Type this command to change the permissions.

sudo chmod 666 /dev/ttyUSB0

Set Up a Catkin Workspace and Install RPLIDAR ROS Packages

Open a terminal window.

Update the package list.

sudo apt-get update

Install the following dependencies.

sudo apt-get install cmake python-catkin-pkg python-empy python-nose python-setuptools libgtest-dev python-rosinstall python-rosinstall-generator python-wstool build-essential git

Create the catkin workspace.

mkdir -p ~/catkin_ws/src

cd ~/catkin_ws/

So we don’t have to source the setup.bash file every time we open a new Linux terminal, let’s add the ~/catkin_ws/devel/setup.bash command to the .bashrc file. Open a new Linux terminal window.

Type the following command to edit the .bashrc text file:

gedit ~/.bashrc

If you don’t have gedit installed, you can install it using this command.

sudo apt-get install gedit

Make sure these two lines are at the end of the .bashrc file.

source /opt/ros/melodic/setup.bash
source ~/catkin_ws/devel/setup.bash

Save the file, and close it.

Type this command in the current terminal (we won’t need to do this again since we’ve added this command to our bash file).

source ~/catkin_ws/devel/setup.bash

Go to the source folder.

cd src

Clone the RPLIDAR ROS package to your src folder.

sudo git clone https://github.com/Slamtec/rplidar_ros.git

Go to the root of the workspace.

cd ~/catkin_ws/

Compile the workspace.

catkin_make

I saw an error that says “C++11 requires space between literal and string macro.”

The error is occurring in the node.cpp file.

Open that file.

cd ~/catkin_ws/src/rplidar_ros/src

sudo chmod +x node.cpp

sudo gedit node.cpp

We go to line 212.

Change this:

ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION"");

To this:

ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:" RPLIDAR_SDK_VERSION "");

Go to the root of the workspace.

cd ~/catkin_ws/

Compile the workspace.

catkin_make

Launch the RPLIDAR launch file.

roslaunch rplidar_ros rplidar.launch

Check out the currently active topics.

Open a new terminal window, and type:

rostopic list

Let’s see what LIDAR data is being published to the /scan ROS topic. Open a new terminal window, and type:

rostopic echo /scan

Here is what you should see.

Keep the terminal window open for the next section.

Run rviz

Now, we will launch rviz, a 3D visualization software program for ROS.

Launch RPLIDAR.

roslaunch rplidar_ros rplidar.launch

Open a new terminal window, and launch rviz.

rviz

Here is the window that you should see.

Change the Fixed Frame to laser.

Click the Add button in the bottom-left of the window.

Select LaserScan, and click OK.

Set the LaserScan topic to /scan.

Increase the size to 0.03 so that you can see the data more easily.

If you see output like this, it means everything is working correctly.

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

Build a Map Using the Hector-SLAM ROS Package

In this section, we will build a map of our environment using the ROS Hector-SLAM package. 

Hector-SLAM is an algorithm that uses laser scan data to create a map. The advantage of Hector-SLAM over other SLAM techniques is that it only requires laser scan data to do its job. It doesn’t need odometry data. 

Hector stands for “Heterogeneous Cooperating Team of Robots”, the name of a robotics research group at TU Darmstadt. The original research paper is available here if you want to learn more about the details (you don’t need to know the details of the algorithm in order to use it in your applications).

SLAM stands for Simultaneous Localization and Mapping. SLAM is a popular technique in which a robot generates a map of an unknown environment (i.e. mapping) while simultaneously keeping track of its position within that map (i.e. localization).

Ok, let’s get started.

Install Qt4

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

sudo apt-get install qt4-qmake qt4-dev-tools

Type Y and press Enter to complete the installation. The whole process should take a few minutes.

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.

Make sure you have an Internet connection.

ping google.com

If you see streaming data, your Internet is connected properly.

Now download the Hector-SLAM package.

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="base_link"/>

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="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->

Change those lines to this (be sure to remove the comment tags (<!– and –>):

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

Save the file, and return to the terminal window.

Type the following command.

cd ~/catkin_ws/src/hector_slam/hector_slam_launch/launch

Open the tutorial.launch file.

gedit tutorial.launch

Find this line (line 7 in my code).

<param name="/use_sim_time" value="true"/>

Change that line to:

<param name="/use_sim_time" value="false"/>

Save the file, and close it.

Open a new terminal window, and type this command:

cd ~/catkin_ws/

Build the packages.

catkin_make

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.

cd ~/catkin_ws/

catkin_make

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

Let’s reboot the computer. I’ll do a complete shutdown then turn the Jetson Nano on again.

sudo shutdown -h now

Launch Mapping

Turn on the Jetson Nano.

Open a new terminal window, and launch RPLIDAR.

cd ~/catkin_ws/

sudo chmod 666 /dev/ttyUSB0

roslaunch rplidar_ros rplidar.launch

Now that the LIDAR is running, let’s start mapping. Open a new terminal, and type the following command:

roslaunch hector_slam_launch tutorial.launch

This command above launches three nodes as well as rviz:

• hector_mapping node 
• hector_trajectory_server 
• hector_geotiff

If you move the LIDAR around the room, make sure that it moves very slowly so that you can create a good map. Map-making works best at slow speeds. Here are some photos of the robot I built. It combines obstacle avoidance and LIDAR-based mapping.

Save the Map

Method 1

Let’s save the map. Open a new terminal window, and type:

rostopic pub syscommand std_msgs/String "savegeotiff"

When you’re done running mapping, type CTRL + C on all terminal windows.

Go to ~/catkin_ws/src/hector_slam/hector_geotiff/maps.

You can find your own map like hector_slam_map_##:##:##.tfw.

Method 2

Another way to save a map is to use a package called map_server. This package will save map data to a yaml and pgm formatted file.

Before you launch the mapping process, you need to install map_server.

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

Create a maps folder. This is where we will store our maps.

mkdir ~/catkin_ws/maps

Launch the mapping process (see Launch Mapping section above).

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

cd ~/catkin_ws/maps

rosrun map_server map_saver -f my_map

Maps will save to the ~/catkin_ws/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..

cd ~/catkin_ws/maps

roscore

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

rosrun map_server map_server my_map.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.

Press CTRL + C to close everything down.

Convert the Map into png Format

To convert the map to a png image file, you can use imagemagick.

sudo apt-get install imagemagick

convert my_map.pgm my_map.png

Edit the Map

To edit the map, I recommend using a program like GIMP.

Install gimp.

sudo apt-get update

sudo apt-get install gimp

Run gimp.

gimp

To load the image, go to File -> Open, and then locate your image.

That’s it. Keep building!

References

This tutorial was especially helpful.