Maps

For a robot, understanding the world around itself is vital for safe and effective navigation. Even as humans, it is difficult to navigate a new place or environment. Imagine the first time you visited the University of Virginia, and the tour guide gave your next set of directions as "Head North until you arrive at Rice Hall". Although in general, these directions might be correct, there would be a very high chance that at some point you would physically not be able to head north and would end up getting lost. The solution to this problem for both humans and robots is to use maps. With maps, a robot can understand the environment, identify obstacles, and plan an effective route through the environment to the goal.

Over the years, there have been many proposed mapping techniques to allow robots to complete there task. For instance, assume a robot does not know the environment, as shown in the Clearpath example. The robot could break the environment up into different cells representing locations in the environment and then populate each cell with the probability that an obstacle lies in that cell (shown in blue) as it drives around. This would allow the robot to keep track of its location and build an understanding of the world around it using its new map. Another example is google maps, which starts with a well-defined environment and thus can much more accurately place obstacles and free space. The final example is from Autoware, which is an open-source self-driving simulator. In this example, we can see that the robot is combining both static maps such as google maps and combining it with sensor data to build a highly accurate representation of the world around it.

For a robot having the ability to use maps and accurately keep track of its location on a map is vital for robot navigation. In this lab, we will implement a mapping server on our drone and use it to navigate the robot to a goal.

Learning Objectives

At the end of this lab, you should understand:

• How to configure and add the ROS map_server to your ROS project
• How to create and use an occupancy grid map
• How to navigate a robot through a map

Before Starting

We will be using an updated version of the fastersim simulator for the remainder of this course. To get started we need to clone and compile this simulator:

$ git clone https://github.com/hildebrandt-carl/labsim.git

$ cd ~/labsim

$ catkin build

The simulator has two main launch files. The first launches the simulator with manual keyboard controls. To use this launch file you can run:

$ source ~/labsim/devel/setup.bash

$ roslaunch flightcontroller fly_manual.launch 

The second launch file launches the simulator without manual controller, and instead uses a path planner you will need to complete.

$ source ~/labsim/devel/setup.bash

$ roslaunch flightcontroller fly_path.launch  

Approach for Navigating a Map

In this lab we are going to get our drone to navigate through the world. We are going to need a map to represent and understand the world and a path planner to take our drone to a target goal. The path planner will also need to make sure the path it creates is collision-free and safe for the drone to fly. To create collision-free safe paths, the path planner needs to know where the obstacles in the world are. If these obstacles are static and well known, a good way to represent them is by using maps.

To start, we will need a way to describe maps. By describing maps independently of any system, we will be able to reuse maps for different systems, and we will be able to change maps to expose our system to different worlds under simulation.

From a design perspective, it makes sense to allocate map handling functionality to a node. That way, if the format of the maps changes, only this node is affected. This node should then be able to consume a map in a specified format and make it available to other nodes as a standardized data structure containing a map.

Once a map is available, we will need an additional node to consume the map data structure and find a path to go from a starting location to a goal location. This node can send commands to the drones onboard controllers (just like we did with the keyboard node). More specifically, our path planner node will transform the map into a graph which can then be searched using any standard graph searching algorithm. The path planner node will publish a trajectory as well as a waypoint that describes where the drone is going at the time.

Note: Notice the advantages of ROS and the advantages of using a modular design. In the future, if we want a new path planner, the only code we need to consider is the path planner node. This same logic holds for both the maps and controllers. Using ROS is not the only way to achieve this, for example, code with well-designed interfaces might accomplish the same thing. However, with ROS you start with easy to implement interfaces which tends to force development into modular easy to maintain building blocks.

Starting Code

Given what we know, we can start to design our system and connect it to the existing system. We know we want to read maps from map files. The map will then be published on a topic for our path planner to read. The path planner will compute a path and publish both a trajectory as well as the correct next position in the trajectory. The position controller (Which is the same node the keyboard manager published to) will fly the drone to the next waypoint. The trajectory is read by the visualizer node and displayed. This is all shown below in the overview (red means unimplemented and blue means implemented already):

Using this design lets see how it links into our existing system. After launching the simulator run RQT graph. You will see the following graph (Note: Uncheck "Leaf topics" and change the dropdown to display "Nodes/Topiscs (All)"):

We first notice that neither the map server or the path planner nodes have been implemented. We will need to do that in this lab. We can see that the visualizer node as well as the position controller nodes already exist. Lets identify what topics they use and how they work.

1) We notice that we are given three drone controllers. The highest level controller is the position_controller_node. If we hover our mouse over this node you will see that it subscribes to /uav/input/position topic. This is the same topic we used with our keyboard node to fly the drone. Thus our path planner node will need to send position commands on the topic /uav/input/position to control our drone and allow it to follow a trajectory.

2) Next, while we were hovering our mouse over the position_controller_node we also noticed that it now publishes to the topic /uav/sensors/atwaypoint topic. This can be used by other nodes to determine if the drone is at the requested position or not.

3) Finally, take notice that the view_node now subscribes to two new topics /uav/trajectory and /map. Nothing publishes to these topics, and in this lab we will need to correct that. We will need to make sure the path planner node publishes to the /uav/trajectory topic and that our map node publishes to the /map topic.

A simplified version of how the system looks is shown below. Note: red is what we need to do in this lab, and the blue has already been done.

The Map Server

Let's start by implementing the map server node. If you look back at the first few labs, you will remember that we covered how many of the nodes that perform common robotic tasks are readily available for download. Mapping is one of those common tasks covered by existing code that has been shared and packaged extensively. One of these available nodes is the map_server node. A quick search for ROS map server will result in the following webpage:

https://wiki.ros.org/map_server

Reading the first line from this wiki we see that this node is exactly what we need: "the map_server ROS Node, which offers map data as a ROS Service". If we read through the Wikipedia page, we will pick out a few main important points:

• Maps are stored as a pair of files (i.e. each map requires two files).
  • The first file is a YAML file that describes the map meta-data.
  • The second file contains the map data as an image file.
• The image file encodes the occupancy data.

Setting up the Map Server

The first thing we need to do is make the map_server node available to our project. To do that we can install the pre-compiled version of the map_server by running the command:

$ sudo apt-get install ros-kinetic-map-server -y

The next thing we will need is a map file to check that the downloaded node works as expected. We will use one of the map files provided in our simulator. Let's start a roscore and check that we are now able to launch the map_server node. To do that open three terminals and run the following commands:

Terminal 1: (remember roslaunch files automatically start a roscore. However we do not have a launch file yet, hence why we need to do this manually):

$ roscore

Terminal 2:

$ rosrun map_server map_server ~/labsim/src/flightcontroller/maps/map_medium0.yaml

>>> [ INFO] [1584732125.415783128]: Loading map from image "/home/robotclass/finalsim/src/flightcontroller/maps/map_medium0.png"

>>> [ INFO] [1584732125.416015567]: Read a 20 X 20 map @ 1.000 m/cell

Terminal 3:

$ rostopic list

>>> /map

>>> /map_metadata

>>> ...

Finally let's look at what the /map topic contains. Echo the topic using:

Terminal 3:

$ rostopic echo /map

>>> header:

>>> ...

>>> info: 

>>> ...

>>> data: [0, 0, 0, ..., 100, ..., 0]

You should see the meta data, the origin and then the data. To understand this we first need to review our understanding of occupancy grids.

Occupancy Grids

An occupancy grid is a probabilistic grid map, where each cell in the map is the probability that an obstacle is in that cell. These maps are useful when we have noisy sensor data, or we only have partial sensor information. Cells where our sensors detect obstacles more often than not or where we don't have any sensor readings will end up with a higher probability of having an obstacle, and our robot will try to avoid them. Cells where obstacles were detected rarely and irregularly (probably due to the sensor noise), will have a lower probability of having an obstacle and our robot needs not to worry about them.

Below is an example of a robot driving around. The robot has eight beam-sensors that can detect whether an obstacle is in a cell or not. Notice how the occupancy grid updates. Gray cells mean there is a 50/50 chance of having an obstacle. The darker a cell gets the higher probability of an obstacle, the lighter a cell gets the less chance there is an obstacle. Note that in this example, the robot did not rely just on a predefined map to determine the location of obstacles but rather uncover them as it navigated through the world. In or case we have a predefined map, and thus obstacles can be marked with 100% probability directly. However, even if a map is provided, it is still useful to use an occupancy grid as some portions of the map may not be fully or clearly defined, causing similar uncertainty. For simplicity in this lab, we will not consider those cases.

Example by Daniel Moriarty: Link

For this lab, we will assume that the map that is given to us is complete and correct and that there are no surprising obstacles. That is, we will design our maps to only have true obstacles (100% chance that there is an obstacle) and true free space (0% chance that there is an obstacle). Using this information, we can start to make sense of the data found on the /map topic. On the /map topic we saw the data was a set of to either 0 or 100, each representing the chance that an obstacle was detected. Note: When considering probability in a strictly mathematical sence, it is a real number between 0 and 1. They calculate the probability in each cell is caculated using p = (255 - x) / 255.0, where x is the pixel value between 0 (white) and 255 (black). However, we notive that in the occupancy grid they represent the probability as an integer between 0 and 100. This is simply the true mathematical probability scaled up by a factor of 100 and converted to an integer. Representing the probability as an integer is done to save on space, as maps can quickly become large and representing each cell with a float would quickly take up a lot of space.

Launching the Map Server to Display Map in the Simulator

Now that we have a map and understand how is represented, we will connect the map_server node to the simulator. We have already installed the node into our ROS development environment, thus to launch it with the simulator, it is just a matter of including it into the launch file. Add the map_server node to fly_path.launch and fly_manual.launch. Use the rosrun command from earlier to infer the name, package, and type. Including arguments to launch files will be addressed later in this lab, and so has been provided. Below is what the final line should look like:

Note: Make sure to add the map_server AFTER path_planner as it is order dependent.

<node name="TODO" pkg="TODO" type="TODO" args="$(find flightcontroller)/maps/map_medium0.yaml" />

Custom Maps

Now that we can load maps, let's change our system so that it can load any given map file. We have provided you with five different map file pairs that can be found here:

ls ~/finalsim/src/flightcontroller/maps

Now, we want to design our system so that maps can be easily and quickly loaded into the simulator.

Adding Arguments to the Launch File

When we added the line to our launch file we hard-coded the argument as follows:

args="$(find flightcontroller)/maps/map_medium0.yaml"

This command first finds the flightcontroller packages path and then hard-codes the /maps/map_medium0.yaml to this path. We can make this dynamic by changing the map name to a launch file argument. To do this, start by replacing the name of the launch file with the argument map_file. We can do that by changing the argument as follows:

args="$(find flightcontroller)/maps/$(arg map_file)"

In this case, we start by finding the path to the package flightcontroller using $(find flightcontroller) and then append the path /maps/ followed by an argument called map_file using $(arg map_file). The final step is to expose the argument map_file such that we can change that variable from the terminal. To do this add the following line to the top of our launch file

<?xml version="1.0"?>

<launch>

  <arg name="map_file" default="map_medium0.yaml"/>

  ...

This declares that we are creating an argument named map_file and that the default value for this variable is map_medium0.yaml.

Let us check that we set up everything correctly by launching the system and not declaring what the map_file variable instance. It should default to testmap0.yaml and we should see what we saw in Checkpoint 1. Launch the simulator using:

roslaunch flightcontroller fly_manual.launch

Finally lets check that we are able to change the argument using:

roslaunch flightcontroller fly_manual.launch map_file:=map_medium1.yaml

Below we have included what each of the different map files should look like in the simulator.

Understanding Map File Formats

Remember when we were reading the map_server wiki page we noticed that they talked about pairs of map files:

• *.yaml -> Describing the metadata of the map
• *.png -> Describing the occupancy grid data file

The second file is an image format. Having this file format is useful as we can visualize the map using any standard image viewer. For instance, in file explorer navigate to the maps folder ~/labsim/src/flightcontroller/maps/. Open one of the map files and you should see:

Let's look inside the map_medium0.yaml file:

map_medium0.yaml

image: map_medium0.png

resolution: 1

origin: [-10.0, -10.0, 0.0]

occupied_thresh: 0.65

free_thresh: 0.196

negate: 0

The file declares 6 attributes, where each is:

• image: Points to the file name of the corresponding map
• resolution: This declares the resolution of our file. i.e. each pixel is 1 meter in real life. Generally the resolution will be much smaller but to keep our simulator light weight we increased the resolution
• origin: This declares which pixel is the origin. In our case each map is only 20 pixels wide, thus the origin is the center [-10, -10].
• occupied_thresh: This tells the map_server and any node using it what probability we use to declare that a cell has an obstacle (or is occupied).
• free_thresh: The threshold under which we assume the cell is obstacle free.
• negate: We can use this to invert the map and read it as white space is obstacles and black space is obstacle free.

Now that we have looked at the YAML file, we understand why the map looked so small when we opened them. It was only 20 pixels large, but each pixel represents a large space.

Creating our own Map

Another benefit of representing maps as images is that we can use standard image processing tools to create our maps by hand. Of course there are other ways of creating maps, such as populating them using the sensors on the robot. However it might be useful to know how to create maps by hand if you want to test certain scenarios. To do this we are going to use a free image processing tool in Linux called Pinta. Pinta is similar to Paint that comes with Windows. Note: If you feel more comfortable in another image processing tool, feel free to use that. Let's start by installing Pinta. To do that we run the following commands:

$ sudo add-apt-repository ppa:pinta-maintainers/pinta-stable

$ sudo apt-get update

$ sudo apt-get install pinta

We can then launch Pinta using the command line

$ pinta

You should screen like this:

Start by resizing the window. We are going to make this map slightly larger so that it is 30 pixels X 30 pixels. You can do that using the drop-down:

• image->resize image->by absolute size

Uncheck the maintain aspect ratio, and then change the width and height to 30 pixels. Zoom in so that you are able to work on the file. Next, under tools, select the pencil so that we can draw our new map. The pencil can be found as shown below:

Finally draw a map. Make sure to leave the central area blank as this is where your drone will spawn. Once you are done drawing your map, save the map in the correct folder using file->save as. Now that we have our new map file, the final step is to create the corresponding YAML file with the map specifications as follows. Save the YAML file using the same name as your image file with the yaml extension.

image: TODO

resolution: 1

origin: [TODO, TODO, 0.0]

occupied_thresh: 0.65

free_thresh: 0.196

negate: 0

Using what you have learned, fill in the TODOs section of the yaml file.

Grids as a Special Type of Graph

Now that we can load maps, the second part of this lab will be to navigate them.

In class, we learned that we could create trajectories through a map by converting the problem into a graph searching problem. If we can turn each position, the robot can visit into a node, and connect those nodes with edges., we can search through the resultant graph to find a path from the current node to our goal node. There is one problem with our setup right now. Our maps are currently represented as a grid.

The good news is that grids can be easily represented as a graph as well. Look at the grid below. You will notice that we can represent each cell as a node, and then add edges to each adjacent cell.

Using this new understanding of grids, we can easily convert our map into a graph. As we know, our map represents free space using 0, and obstacles using 100. Thus for cells that have the value 0, there are edges to all adjacent nodes that also have a value of 0. Moreover, because we are using a drone, we assume that we can fly diagonally, and thus we can even connect all diagonal edges. Thus our occupancy grid can be converted into a graph as shown below:

Now that we know how to treat our occupancy grid as a graph, we can start to implement the graph searching algorithm, allowing our drone to navigate through our maps.

Finding a Path

For the final part of this lab, we will use the map to plan a route from our drone's position to a set goal. We will be using an A* search to find the path through the map. A* works by favoring nodes that are better according to some heuristic. That heuristic may any function for example, distance to the goal, time to the goal, or even force applied to the vehicle. In our case, we will define the heuristic as the euclidian distance to the goal. An example of the A* algorithm is shown below. We can see that it favors nodes that are closer to the goal. We see this as it first explores nodes leading you to the goal. Next, once it hits the obstacle, it slowly expands until it finds a path around the obstacle and then continues towards the goal.

Example by Wikipedia: Link

To start the implementation of our algorithm, we will create a node which controls the flow of our implementation. The node will subscribe to the map, current position, and goal. Once the node has this information, it will search the graph for the shortest path and then publish a requested position and a trajectory.

The code for this node path_planner is given and fully implemented in the simple_control package. To understand how it works, we can take a look at the pseudocode below. First, we initialize variables we will be using in the algorithm in lines 1 and 2. Then we check if we do not have a plan in line 4. Once we have a map, goal, and position, we use the A* algorithm to find this plan in lines 5-6. Then in line 11 we get the next point in the trajectory and publish it in line 15. Line 12 checks if we have reached the requested position and if there are still more points in the trajectory. If these are met, we increment the point we want to publish in line 13.

Your job will be to implement the A* search algorithm class (Algorithm shown below). You will notice that inside the given path_planner node in the simple_control package, you can see the following lines.

from astar_class import AStarPlanner

...

astar = AStarPlanner(safe_distance=1)

trajectory = astar.plan(self.map, self.drone_position, self.goal_position)

These lines create an astar class object and then use the member function plan to create the trajectory. Your job will be to complete that python class astar_class.py in the src folder of the simple_control package. The skeleton code for this class is shown below (Note: There are more details in the actual file, this has been shortened for brevity):

...

class AStarPlanner:

  # Initilize any variables

  def __init__(self, safe_distance=1):

    ...

  # Find a plan through the map from drone_position to goal_position

  def plan(self, map_data, drone_position, goal_position):

    # Validate the data

    self.validate_data(map_data, drone_position, goal_position)

    # Expand the obstacles by the safety factor

    map_data = self.expand_obstacles(map_data, self.safe_distance)

    ...

    # TODO

    ...

  # Get the children nodes for the current node

  def get_neighbors(self, node_in, map_in):

    ...

  # Validate the incoming data

  def validate_data(self, map_data, drone_position, goal_position):

    ...

  # Expand the obstacles by distance so you do not hit one

  def expand_obstacles(self, map_data, distance):

    ...

Inside the skeleton code we have implemented three functions for you. The first function is get_neighbors. This function takes in a nodes position and the map and returns all the neighbors of that node. The second is called validate_data. This function is used for asserting that all data passed to the function is valid. The third function is called expand_obstacles, which expands all obstacles by a set value to give us more margin around the obstacles so we do not fly into any obstacle accidentally while traversing the map. The function should return a list of lists. For instance to traverse from the point [15, 15] to [13, 14] you will return the trajectory [[15, 15], [14, 14], [13, 14]].

Note: There are many ways to implement the A* search algorithm. Python has a wide range of data structures you can use for instance PriorityQueues, Lists, Dictionaries. For this reason the code has been left bare, to allow you to implement the algorithm however you see fit. We implemented one version using two lists to keep track of the frontier and nodes which had been visited. This method required us to manually short each list. The second implementation used priority queues and meant we spent more time implementing functions to check whether certain nodes where in the queue or not. Use whichever method you feel most comfortable with.

frontier = PriorityQueue()

frontier.put(start, 0)

came_from = {}

cost_so_far = {}

came_from[start] = None

cost_so_far[start] = 0

while not frontier.empty():

  current = frontier.get()

  if current == goal:

    break

  for next in graph.neighbors(current):

    new_cost = cost_so_far[current] + graph.cost(current, next)

    if next not in cost_so_far or new_cost < cost_so_far[next]:

      cost_so_far[next] = new_cost

      priority = new_cost + distance(goal, next)

      frontier.put(next, priority)

      came_from[next] = current

trajectory = build_trajectory(current)

Testing your Search Algorithm

We have provided you with unit tests to test your python class. Unit tests are used in software testing and development to check if a section of code, known as a unit, meets specific design criteria for a unique input. As we are testing a python class, we will use Python's built-in unit tests. The unit test works by creating each test as a method with the name test_<somename>. Moreover, you can define special functions setUp and tearDown, which will run at the start and end of each test. For more information, you can view the unit test documentation. You can view the test class we provided in ~/labsim/src/simple_control/src/test_astar.py. We provide four test cases:

• test_straight_x: Tests if the goal is placed in a straight line along the x-axis with no obstacles. We expect the trajectory to move directly there in a straight line.
• test_straight_y: Tests if the goal is placed in a straight line along the x-axis with no obstacles. We expect the trajectory to move directly there in a straight line.
• test_diagonal: Tests if the goal is placed along a diagonal line along both the x and y-axis with no obstacles. We expect the trajectory to move directly there along a diagonal line.
• test_obstacle: Tests if the goal is placed in a straight line along the y-axis with an obstacle in between. We expect the trajectory to lead around the obstacle taking the path with the shortest length to the goal.

You can use these unit tests to help debug your A* search class. We strongly recommend that you add more unit tests to check if your implementation is working correctly. For example, what about the corner case where the path needs to backtrack out of an enclosed space? To run the tests, you run the following command:

$ cd ~/labsim/src/simple_control/src

$ python2 test_astar.py

Note: Make sure to use Python2 as this is the version of python that ROS uses. If all tests pass, you should have the following output:

>>>....

>>> ----------------------------------------------------------------------

>>> Ran 4 tests in 0.062s

Once you are sure your code works you should be able to launch the simulator and send it a goal. You can do that as follows:

Terminal 1:

$ roslaunch flightcontroller fly_path.launch map_file:=map_medium0.yaml

Terminal 2:

$ rostopic pub /uav/input/goal geometry_msgs/Vector3 '{x: -2, y: -1, z: 2.5}'

The second terminal will publish the goal, which will invoke the A* planner. Note: You might notice that we are requesting a negative x and y as a goal. In our python unit tests, these would have returned errors, as you cant request a negative pixel value. The reason we can do this is because of a transformation from a coordinate system used by the map to the coordinate system used by our system. The center of a map, which is 20X20 pixels wide, is [10,10]. However, the drone starts in the center of the simulation, which is [0,0]. Thus when we request the position '{x: -2, y: -1}', this is transformed to the map coordinate system, which would be [8,9]. If this does not make sense, do not worry, this is done for you in this lab, and will be covered in more detail in the lab 8.

To Check

To Check

1. Launch the simulator and load in a custom map using command line arguments
2. Verify that your path planning works
   1. Show that the current unit tests pass. Explain any other unit tests that you added and why you added them.
   2. Show that you can compute trajectories and that the drone will traverse the trajectories.

Extra (1 pt)

In this lab you can get extra credit. To get extra credit implement a new class bfs_class.py. This class should be similar to the A* class except it should use BFS as the underlying search algorithm and use it to navigate your drone through the map. What are the differences between A* and BFS in a scenario of your choice?