Overview

What the code does

After collecting LIDAR data using a Turtlebot, and creating a map file from this data using the pre-built ROS Package gmapping, our package finds the optimal path between two points on the map and publishes commands to control a Turtlebot in real life and in simulation.

Architecture Diagram

How the code works

ROS Navigation Packages

We used the ROS package gmapping to make maps from LIDAR data after driving the TurtleBot around a space. This package generated a .pgm and a .yaml file, which we later used for the simulation.

A* algorithm

We start with making a dictionary containing all of the points that are 'obstacles', or the pixels that are black or grey in the image. Then, starting at the initial pixel that the robot is at, we look at the four adjacent pixels. For each one that is available, we add it to a dictionary of open spots and calculate the G value, (the cost to get to that spot), the h value (the euclidean distance estimated to get to the goal), and the f value (the sum of the g and h values). From there, we select the pixel with the lowest F value to search next. If many F values are the same, we prioritize pixels that were recently checked. We add the pixel we just checked to the closed list of pixels that aren't allowed. Then we repeat this process until we arrive at the goal. To get the path, we simply back track, following the pixels with the lowest cost values.

Converting the Path to Velocities

We transformed this list of pixels into a series of velocity vectors by connecting consecutive points. Since the distance between two consecutive pixels is very small, it is time consuming and unnecessary to drive the robot in small steps where the linear and angular velocity is constant for multiple time steps. Therefore, we decrease the resolution of the image and instead compute the Euclidean distance of every 5 pixels to allow the code to run faster.

Publishing the Velocities in ROS

First we initiate a ROS publisher that will publish the velocities to the topic that handles velocity. Then we have a loop that runs through each of the velocities in the given list. If the index is odd, then the element is a linear velocity and is handled as such. If it is even, the element is an angular velocity and is handled as such. Since the Turtlebot naturally stops after a certain amount of time has passed since the last publish. As such, the velocity has to be continuously published.

Stage simulation and Using Real Robot

To run the robot in simulation, we created a package based on turtlebot_stage which uses the simulation/visualization software RVIZ. Based on where the user clicks on the map, commands will be generated for the robot and published to the ROS topic /cmd_vel_mux/input_navi. To run the robot in real life, we used the package turtlebot_bringup which set up most of the low-level controls for the robot.