PaparazziUAV - User contributions [en]

AR Drone 2/Mapping

2013-01-31T12:51:59Z

Koudshoorn: /* DELETE_FRONTIERS */

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum. Note however that, while true for some, not all states shift to the next after one iteration. In some cases, FIND_PATHS for example, it will take a few iterations before it will shift.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scanning===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===
In "Search for frontiers", we obviously search for frontiers. As the paper of Frontier-Based Exploration explains, we will use these frontiers to deduct our next goal to travel to. As can be seen in the statediagram, we created substates for "Search for frontiers" (DELETE_FRONTIERS, FIND_FRONTIER_NODES, BUILD_FRONTIERGRID and CLUSTER_FRONTIERS) for reasons explained earlier.

==== DELETE_FRONTIERS====
In the mapping algorithm, we maintain a list of frontierpoints (2D points to represent position) for the sake of speed. That way, we only have to check the area, obtained during state Scanning, for new frontiers instead of going over the whole map. But because of that, we also need to remove any frontierpoints when they´re not frontiers anymore. DELETE_FRONTIERS is the state where every frontierpoint in the list is checked, to see whether it's still a frontier or not.

==== FIND_FRONTIER_NODES====
When all the outdated frontiers are deleted, we need to add the newly created frontiers. This happens in FIND_FRONTIER_NODES. Because the area that got updated in Scanning is the only area that got updated, we only have to search for new frontierpoints in there. Those frontierpoints will then be added to the list.

==== BUILD_FRONTIERGRID====
With the frontierpoint list we build a 2D grid representation of the map and fill it with frontiers on their respected positions (positions without frontiers are null pointers). This grid will be used in the next state to cluster them all together.

==== CLUSTER_FRONTIERS====
In this state, the frontiers will be clustered together using a [http://en.wikipedia.org/wiki/Disjoint-set_data_structure Union-Find Algorithm]. With the list of frontierpoints, we can check the surrounding tiles of that position within the frontiergrid to find neighbouring frontiers. These neighbours are then clustered together. When the size of a cluster reaches a certain threshold, it is added to a list of clustered frontiers. The threshold has two advantages. One of them is that the drone will never visit frontiers that are too small to be of any use. The other advantage is to keep the amount of checks for redundant clusters in the list low. You see, when two clusters, that aren't the same, have been added to the list and later on these two clusters have formed 1 cluster, you'll have two of the same cluster in the list and one will have to be removed. With the threshold it will happen less often that two clusters inside the list will be unified.

=== Pick best frontier===
Having obtained several clusters of frontiers, we can now use these to deduct our next goal location. By normalizing the size of the frontiers and travelling distance towards them, we can pick the frontier with the optimum of small travelling distance and large size. However, before we can know the travelling distance towards a frontier we need to know the path towards it. For this we apply pathfinding in the FIND_PATHS state using the [http://www.policyalmanac.org/games/aStarTutorial.htm A* algorithm], we choose this algorithm mainly for it's simplicity. To make travelling along the path easier, we trim the found paths to a set of waypoints (not tile by tile). After all the paths have been found, the best frontier will be picked in the FIND_BEST_FRONTIER state.

=== Travel===
Now knowing where the drone should be headed to, we can start following the path we calculated earlier. Since the path exists out of waypoints, the easiest way of travelling is by turning towards the waypoint (TURN_TO_WAYPOINT), followed by moving forward until waypoint is reached (TRAVEL_TO_WAYPOINT). When the waypoint is reached you take the next waypoint and travel to that one, until there are no more waypoints left and you should have arrived at the goal.

== Integration with Paparazzi==

Since we had a hard time to get the AR.Drone2 flying, we ended up with too little time to integrate the mapping algorithm with Paparazzi. We did however work out how we wanted to integrate it. Our ideas will be explained in this section, just keep in mind that most of it is speculation.

For the mapping algorithm to work with Paparazzi, we need to be able to do a few things:
* Keep track of position and heading
* Pilot the AR.Drone2
* Read out the [[AR_Drone_2/Multidirectional_distance_measurement| distance sensor data]]
* Let Paparazzi call the algorithm in an iterative way

The first and third bullets are necessary to be able to know which maptiles to update. Keeping track of position and heading is also important for being able to calculate the next action to be taken by the drone (it's hard to figure out how to get somewhere if you don't know where you are). The second bullet is needed to be able to fly to where we want, how we want. The last thing we need is to execute the mapping algorithm, since Paparazzi is the main program, it will have to execute the mapping algorithm (and not the other way around).

=== Keep track of position and heading===
Paparazzi itself keeps track of a position and heading according to gps (this is all located in sw/airborne/state.h). Altough the heading is no problem (you can keep the map pointed north that way), it's hard to convert gps coördinates into map coördinates. But there's another solution to this: if we take the starting position as [0, 0] we can then update the position by integrating the velocity every iteration. Altough the outcome won't be 100% correct, the error will be kept to a minimum with a high enough update rate. In case velocity isn't updated fast enough we can also try using the acceleration from the accelerometers to update position.

=== Pilot the AR.Drone2===
Flying with Paparazzi currently works in two different ways: autonomous with a [[Flight_Plans| flight plan]] or on radio control with support of the autopilot until a certain degree. Flight plan based will mean that we'd have to be able to update the flight plan while flying from within the mapping algorithm. This is a hard thing to do since the flight plan is located in the ground station and the mapping algorithm will be working on the AR.Drone2 (the airborne part). Though it would be very nice and would also take away the first problem of keeping track of your own position (the flight plan can coöperate with relative x and y values).

The second option is a lot easier to implement, altough more prone to errors in the positioning. When using radio control, Paparazzi sends the radio control values from the ground station to the airborne. There, a struct gets filled with the values, which in turn gets handled by the autopilot. By using the right [[Rotorcraft_Configuration#Autopilot_modes| autopilot mode]] we can fly stable to the locations we want (AP_MODE_HOVER_Z_HOLD for yawing and AP_MODE_ATTITUDE_Z_HOLD for moving). Note however that by using the radio control struct you can only use 3 different autopilot modes (these are described in the [[Rotorcraft_Configuration| airframe]]).

=== Read out the distance sensor data===
To keep it simple, we wanted to directly access the data from the sensors. It is ofcourse possible to integrate the sensors with Paparazzi as well, but Paparazzi itself has no need for distance measurement.

=== Let Paparazzi call the algorithm in an iterative way===
To let Paparazzi use the mapping algorithm, we can implement mapping as a [[Subsystem| subsystem]]. That way mapping can be added optionally to the airframe (just like gps is optional). Since the mapping algorithm is to be called in an iterative way, we can tell the main control loop to call the algorithm and we're done.

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-31T12:51:45Z

Koudshoorn: /* FIND_FRONTIER_NODES */

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum. Note however that, while true for some, not all states shift to the next after one iteration. In some cases, FIND_PATHS for example, it will take a few iterations before it will shift.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scanning===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===
In "Search for frontiers", we obviously search for frontiers. As the paper of Frontier-Based Exploration explains, we will use these frontiers to deduct our next goal to travel to. As can be seen in the statediagram, we created substates for "Search for frontiers" (DELETE_FRONTIERS, FIND_FRONTIER_NODES, BUILD_FRONTIERGRID and CLUSTER_FRONTIERS) for reasons explained earlier.

==== DELETE_FRONTIERS====
In the mapping algorithm, we maintain a list of frontierpoints (2D points to represent position) for the sake of speed. That way, we only have to check the area, obtained during state [[#subsection Scanning|Scanning]], for new frontiers instead of going over the whole map. But because of that, we also need to remove any frontierpoints when they´re not frontiers anymore. DELETE_FRONTIERS is the state where every frontierpoint in the list is checked, to see whether it's still a frontier or not.

==== FIND_FRONTIER_NODES====
When all the outdated frontiers are deleted, we need to add the newly created frontiers. This happens in FIND_FRONTIER_NODES. Because the area that got updated in Scanning is the only area that got updated, we only have to search for new frontierpoints in there. Those frontierpoints will then be added to the list.

==== BUILD_FRONTIERGRID====
With the frontierpoint list we build a 2D grid representation of the map and fill it with frontiers on their respected positions (positions without frontiers are null pointers). This grid will be used in the next state to cluster them all together.

==== CLUSTER_FRONTIERS====
In this state, the frontiers will be clustered together using a [http://en.wikipedia.org/wiki/Disjoint-set_data_structure Union-Find Algorithm]. With the list of frontierpoints, we can check the surrounding tiles of that position within the frontiergrid to find neighbouring frontiers. These neighbours are then clustered together. When the size of a cluster reaches a certain threshold, it is added to a list of clustered frontiers. The threshold has two advantages. One of them is that the drone will never visit frontiers that are too small to be of any use. The other advantage is to keep the amount of checks for redundant clusters in the list low. You see, when two clusters, that aren't the same, have been added to the list and later on these two clusters have formed 1 cluster, you'll have two of the same cluster in the list and one will have to be removed. With the threshold it will happen less often that two clusters inside the list will be unified.

=== Pick best frontier===
Having obtained several clusters of frontiers, we can now use these to deduct our next goal location. By normalizing the size of the frontiers and travelling distance towards them, we can pick the frontier with the optimum of small travelling distance and large size. However, before we can know the travelling distance towards a frontier we need to know the path towards it. For this we apply pathfinding in the FIND_PATHS state using the [http://www.policyalmanac.org/games/aStarTutorial.htm A* algorithm], we choose this algorithm mainly for it's simplicity. To make travelling along the path easier, we trim the found paths to a set of waypoints (not tile by tile). After all the paths have been found, the best frontier will be picked in the FIND_BEST_FRONTIER state.

=== Travel===
Now knowing where the drone should be headed to, we can start following the path we calculated earlier. Since the path exists out of waypoints, the easiest way of travelling is by turning towards the waypoint (TURN_TO_WAYPOINT), followed by moving forward until waypoint is reached (TRAVEL_TO_WAYPOINT). When the waypoint is reached you take the next waypoint and travel to that one, until there are no more waypoints left and you should have arrived at the goal.

== Integration with Paparazzi==

Since we had a hard time to get the AR.Drone2 flying, we ended up with too little time to integrate the mapping algorithm with Paparazzi. We did however work out how we wanted to integrate it. Our ideas will be explained in this section, just keep in mind that most of it is speculation.

For the mapping algorithm to work with Paparazzi, we need to be able to do a few things:
* Keep track of position and heading
* Pilot the AR.Drone2
* Read out the [[AR_Drone_2/Multidirectional_distance_measurement| distance sensor data]]
* Let Paparazzi call the algorithm in an iterative way

The first and third bullets are necessary to be able to know which maptiles to update. Keeping track of position and heading is also important for being able to calculate the next action to be taken by the drone (it's hard to figure out how to get somewhere if you don't know where you are). The second bullet is needed to be able to fly to where we want, how we want. The last thing we need is to execute the mapping algorithm, since Paparazzi is the main program, it will have to execute the mapping algorithm (and not the other way around).

=== Keep track of position and heading===
Paparazzi itself keeps track of a position and heading according to gps (this is all located in sw/airborne/state.h). Altough the heading is no problem (you can keep the map pointed north that way), it's hard to convert gps coördinates into map coördinates. But there's another solution to this: if we take the starting position as [0, 0] we can then update the position by integrating the velocity every iteration. Altough the outcome won't be 100% correct, the error will be kept to a minimum with a high enough update rate. In case velocity isn't updated fast enough we can also try using the acceleration from the accelerometers to update position.

=== Pilot the AR.Drone2===
Flying with Paparazzi currently works in two different ways: autonomous with a [[Flight_Plans| flight plan]] or on radio control with support of the autopilot until a certain degree. Flight plan based will mean that we'd have to be able to update the flight plan while flying from within the mapping algorithm. This is a hard thing to do since the flight plan is located in the ground station and the mapping algorithm will be working on the AR.Drone2 (the airborne part). Though it would be very nice and would also take away the first problem of keeping track of your own position (the flight plan can coöperate with relative x and y values).

The second option is a lot easier to implement, altough more prone to errors in the positioning. When using radio control, Paparazzi sends the radio control values from the ground station to the airborne. There, a struct gets filled with the values, which in turn gets handled by the autopilot. By using the right [[Rotorcraft_Configuration#Autopilot_modes| autopilot mode]] we can fly stable to the locations we want (AP_MODE_HOVER_Z_HOLD for yawing and AP_MODE_ATTITUDE_Z_HOLD for moving). Note however that by using the radio control struct you can only use 3 different autopilot modes (these are described in the [[Rotorcraft_Configuration| airframe]]).

=== Read out the distance sensor data===
To keep it simple, we wanted to directly access the data from the sensors. It is ofcourse possible to integrate the sensors with Paparazzi as well, but Paparazzi itself has no need for distance measurement.

=== Let Paparazzi call the algorithm in an iterative way===
To let Paparazzi use the mapping algorithm, we can implement mapping as a [[Subsystem| subsystem]]. That way mapping can be added optionally to the airframe (just like gps is optional). Since the mapping algorithm is to be called in an iterative way, we can tell the main control loop to call the algorithm and we're done.

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-31T12:51:04Z

Koudshoorn: /* DELETE_FRONTIERS */

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum. Note however that, while true for some, not all states shift to the next after one iteration. In some cases, FIND_PATHS for example, it will take a few iterations before it will shift.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scanning===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===
In "Search for frontiers", we obviously search for frontiers. As the paper of Frontier-Based Exploration explains, we will use these frontiers to deduct our next goal to travel to. As can be seen in the statediagram, we created substates for "Search for frontiers" (DELETE_FRONTIERS, FIND_FRONTIER_NODES, BUILD_FRONTIERGRID and CLUSTER_FRONTIERS) for reasons explained earlier.

==== DELETE_FRONTIERS====
In the mapping algorithm, we maintain a list of frontierpoints (2D points to represent position) for the sake of speed. That way, we only have to check the area, obtained during state [[#subsection Scanning|Scanning]], for new frontiers instead of going over the whole map. But because of that, we also need to remove any frontierpoints when they´re not frontiers anymore. DELETE_FRONTIERS is the state where every frontierpoint in the list is checked, to see whether it's still a frontier or not.

==== FIND_FRONTIER_NODES====
When all the outdated frontiers are deleted, we need to add the newly created frontiers. This happens in FIND_FRONTIER_NODES. Because the area that got updated in [[#subsection Scanning|SCANNING]] is the only area that got updated, we only have to search for new frontierpoints in there. Those frontierpoints will then be added to the list.

==== BUILD_FRONTIERGRID====
With the frontierpoint list we build a 2D grid representation of the map and fill it with frontiers on their respected positions (positions without frontiers are null pointers). This grid will be used in the next state to cluster them all together.

==== CLUSTER_FRONTIERS====
In this state, the frontiers will be clustered together using a [http://en.wikipedia.org/wiki/Disjoint-set_data_structure Union-Find Algorithm]. With the list of frontierpoints, we can check the surrounding tiles of that position within the frontiergrid to find neighbouring frontiers. These neighbours are then clustered together. When the size of a cluster reaches a certain threshold, it is added to a list of clustered frontiers. The threshold has two advantages. One of them is that the drone will never visit frontiers that are too small to be of any use. The other advantage is to keep the amount of checks for redundant clusters in the list low. You see, when two clusters, that aren't the same, have been added to the list and later on these two clusters have formed 1 cluster, you'll have two of the same cluster in the list and one will have to be removed. With the threshold it will happen less often that two clusters inside the list will be unified.

=== Pick best frontier===
Having obtained several clusters of frontiers, we can now use these to deduct our next goal location. By normalizing the size of the frontiers and travelling distance towards them, we can pick the frontier with the optimum of small travelling distance and large size. However, before we can know the travelling distance towards a frontier we need to know the path towards it. For this we apply pathfinding in the FIND_PATHS state using the [http://www.policyalmanac.org/games/aStarTutorial.htm A* algorithm], we choose this algorithm mainly for it's simplicity. To make travelling along the path easier, we trim the found paths to a set of waypoints (not tile by tile). After all the paths have been found, the best frontier will be picked in the FIND_BEST_FRONTIER state.

=== Travel===
Now knowing where the drone should be headed to, we can start following the path we calculated earlier. Since the path exists out of waypoints, the easiest way of travelling is by turning towards the waypoint (TURN_TO_WAYPOINT), followed by moving forward until waypoint is reached (TRAVEL_TO_WAYPOINT). When the waypoint is reached you take the next waypoint and travel to that one, until there are no more waypoints left and you should have arrived at the goal.

== Integration with Paparazzi==

Since we had a hard time to get the AR.Drone2 flying, we ended up with too little time to integrate the mapping algorithm with Paparazzi. We did however work out how we wanted to integrate it. Our ideas will be explained in this section, just keep in mind that most of it is speculation.

For the mapping algorithm to work with Paparazzi, we need to be able to do a few things:
* Keep track of position and heading
* Pilot the AR.Drone2
* Read out the [[AR_Drone_2/Multidirectional_distance_measurement| distance sensor data]]
* Let Paparazzi call the algorithm in an iterative way

The first and third bullets are necessary to be able to know which maptiles to update. Keeping track of position and heading is also important for being able to calculate the next action to be taken by the drone (it's hard to figure out how to get somewhere if you don't know where you are). The second bullet is needed to be able to fly to where we want, how we want. The last thing we need is to execute the mapping algorithm, since Paparazzi is the main program, it will have to execute the mapping algorithm (and not the other way around).

=== Keep track of position and heading===
Paparazzi itself keeps track of a position and heading according to gps (this is all located in sw/airborne/state.h). Altough the heading is no problem (you can keep the map pointed north that way), it's hard to convert gps coördinates into map coördinates. But there's another solution to this: if we take the starting position as [0, 0] we can then update the position by integrating the velocity every iteration. Altough the outcome won't be 100% correct, the error will be kept to a minimum with a high enough update rate. In case velocity isn't updated fast enough we can also try using the acceleration from the accelerometers to update position.

=== Pilot the AR.Drone2===
Flying with Paparazzi currently works in two different ways: autonomous with a [[Flight_Plans| flight plan]] or on radio control with support of the autopilot until a certain degree. Flight plan based will mean that we'd have to be able to update the flight plan while flying from within the mapping algorithm. This is a hard thing to do since the flight plan is located in the ground station and the mapping algorithm will be working on the AR.Drone2 (the airborne part). Though it would be very nice and would also take away the first problem of keeping track of your own position (the flight plan can coöperate with relative x and y values).

The second option is a lot easier to implement, altough more prone to errors in the positioning. When using radio control, Paparazzi sends the radio control values from the ground station to the airborne. There, a struct gets filled with the values, which in turn gets handled by the autopilot. By using the right [[Rotorcraft_Configuration#Autopilot_modes| autopilot mode]] we can fly stable to the locations we want (AP_MODE_HOVER_Z_HOLD for yawing and AP_MODE_ATTITUDE_Z_HOLD for moving). Note however that by using the radio control struct you can only use 3 different autopilot modes (these are described in the [[Rotorcraft_Configuration| airframe]]).

=== Read out the distance sensor data===
To keep it simple, we wanted to directly access the data from the sensors. It is ofcourse possible to integrate the sensors with Paparazzi as well, but Paparazzi itself has no need for distance measurement.

=== Let Paparazzi call the algorithm in an iterative way===
To let Paparazzi use the mapping algorithm, we can implement mapping as a [[Subsystem| subsystem]]. That way mapping can be added optionally to the airframe (just like gps is optional). Since the mapping algorithm is to be called in an iterative way, we can tell the main control loop to call the algorithm and we're done.

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-31T12:50:27Z

Koudshoorn: /* CLUSTER_FRONTIERS */

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum. Note however that, while true for some, not all states shift to the next after one iteration. In some cases, FIND_PATHS for example, it will take a few iterations before it will shift.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scanning===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===
In "Search for frontiers", we obviously search for frontiers. As the paper of Frontier-Based Exploration explains, we will use these frontiers to deduct our next goal to travel to. As can be seen in the statediagram, we created substates for "Search for frontiers" (DELETE_FRONTIERS, FIND_FRONTIER_NODES, BUILD_FRONTIERGRID and CLUSTER_FRONTIERS) for reasons explained earlier.

==== DELETE_FRONTIERS====
In the mapping algorithm, we maintain a list of frontierpoints (2D points to represent position) for the sake of speed. That way, we only have to check the area, obtained during state [[#subsection SCANNING|SCANNING]], for new frontiers instead of going over the whole map. But because of that, we also need to remove any frontierpoints when they´re not frontiers anymore. DELETE_FRONTIERS is the state where every frontierpoint in the list is checked, to see whether it's still a frontier or not.

==== FIND_FRONTIER_NODES====
When all the outdated frontiers are deleted, we need to add the newly created frontiers. This happens in FIND_FRONTIER_NODES. Because the area that got updated in [[#subsection Scanning|SCANNING]] is the only area that got updated, we only have to search for new frontierpoints in there. Those frontierpoints will then be added to the list.

==== BUILD_FRONTIERGRID====
With the frontierpoint list we build a 2D grid representation of the map and fill it with frontiers on their respected positions (positions without frontiers are null pointers). This grid will be used in the next state to cluster them all together.

==== CLUSTER_FRONTIERS====
In this state, the frontiers will be clustered together using a [http://en.wikipedia.org/wiki/Disjoint-set_data_structure Union-Find Algorithm]. With the list of frontierpoints, we can check the surrounding tiles of that position within the frontiergrid to find neighbouring frontiers. These neighbours are then clustered together. When the size of a cluster reaches a certain threshold, it is added to a list of clustered frontiers. The threshold has two advantages. One of them is that the drone will never visit frontiers that are too small to be of any use. The other advantage is to keep the amount of checks for redundant clusters in the list low. You see, when two clusters, that aren't the same, have been added to the list and later on these two clusters have formed 1 cluster, you'll have two of the same cluster in the list and one will have to be removed. With the threshold it will happen less often that two clusters inside the list will be unified.

=== Pick best frontier===
Having obtained several clusters of frontiers, we can now use these to deduct our next goal location. By normalizing the size of the frontiers and travelling distance towards them, we can pick the frontier with the optimum of small travelling distance and large size. However, before we can know the travelling distance towards a frontier we need to know the path towards it. For this we apply pathfinding in the FIND_PATHS state using the [http://www.policyalmanac.org/games/aStarTutorial.htm A* algorithm], we choose this algorithm mainly for it's simplicity. To make travelling along the path easier, we trim the found paths to a set of waypoints (not tile by tile). After all the paths have been found, the best frontier will be picked in the FIND_BEST_FRONTIER state.

=== Travel===
Now knowing where the drone should be headed to, we can start following the path we calculated earlier. Since the path exists out of waypoints, the easiest way of travelling is by turning towards the waypoint (TURN_TO_WAYPOINT), followed by moving forward until waypoint is reached (TRAVEL_TO_WAYPOINT). When the waypoint is reached you take the next waypoint and travel to that one, until there are no more waypoints left and you should have arrived at the goal.

== Integration with Paparazzi==

Since we had a hard time to get the AR.Drone2 flying, we ended up with too little time to integrate the mapping algorithm with Paparazzi. We did however work out how we wanted to integrate it. Our ideas will be explained in this section, just keep in mind that most of it is speculation.

For the mapping algorithm to work with Paparazzi, we need to be able to do a few things:
* Keep track of position and heading
* Pilot the AR.Drone2
* Read out the [[AR_Drone_2/Multidirectional_distance_measurement| distance sensor data]]
* Let Paparazzi call the algorithm in an iterative way

The first and third bullets are necessary to be able to know which maptiles to update. Keeping track of position and heading is also important for being able to calculate the next action to be taken by the drone (it's hard to figure out how to get somewhere if you don't know where you are). The second bullet is needed to be able to fly to where we want, how we want. The last thing we need is to execute the mapping algorithm, since Paparazzi is the main program, it will have to execute the mapping algorithm (and not the other way around).

=== Keep track of position and heading===
Paparazzi itself keeps track of a position and heading according to gps (this is all located in sw/airborne/state.h). Altough the heading is no problem (you can keep the map pointed north that way), it's hard to convert gps coördinates into map coördinates. But there's another solution to this: if we take the starting position as [0, 0] we can then update the position by integrating the velocity every iteration. Altough the outcome won't be 100% correct, the error will be kept to a minimum with a high enough update rate. In case velocity isn't updated fast enough we can also try using the acceleration from the accelerometers to update position.

=== Pilot the AR.Drone2===
Flying with Paparazzi currently works in two different ways: autonomous with a [[Flight_Plans| flight plan]] or on radio control with support of the autopilot until a certain degree. Flight plan based will mean that we'd have to be able to update the flight plan while flying from within the mapping algorithm. This is a hard thing to do since the flight plan is located in the ground station and the mapping algorithm will be working on the AR.Drone2 (the airborne part). Though it would be very nice and would also take away the first problem of keeping track of your own position (the flight plan can coöperate with relative x and y values).

The second option is a lot easier to implement, altough more prone to errors in the positioning. When using radio control, Paparazzi sends the radio control values from the ground station to the airborne. There, a struct gets filled with the values, which in turn gets handled by the autopilot. By using the right [[Rotorcraft_Configuration#Autopilot_modes| autopilot mode]] we can fly stable to the locations we want (AP_MODE_HOVER_Z_HOLD for yawing and AP_MODE_ATTITUDE_Z_HOLD for moving). Note however that by using the radio control struct you can only use 3 different autopilot modes (these are described in the [[Rotorcraft_Configuration| airframe]]).

=== Read out the distance sensor data===
To keep it simple, we wanted to directly access the data from the sensors. It is ofcourse possible to integrate the sensors with Paparazzi as well, but Paparazzi itself has no need for distance measurement.

=== Let Paparazzi call the algorithm in an iterative way===
To let Paparazzi use the mapping algorithm, we can implement mapping as a [[Subsystem| subsystem]]. That way mapping can be added optionally to the airframe (just like gps is optional). Since the mapping algorithm is to be called in an iterative way, we can tell the main control loop to call the algorithm and we're done.

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-31T12:49:59Z

Koudshoorn: /* Pick best frontier */

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum. Note however that, while true for some, not all states shift to the next after one iteration. In some cases, FIND_PATHS for example, it will take a few iterations before it will shift.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scanning===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===
In "Search for frontiers", we obviously search for frontiers. As the paper of Frontier-Based Exploration explains, we will use these frontiers to deduct our next goal to travel to. As can be seen in the statediagram, we created substates for "Search for frontiers" (DELETE_FRONTIERS, FIND_FRONTIER_NODES, BUILD_FRONTIERGRID and CLUSTER_FRONTIERS) for reasons explained earlier.

==== DELETE_FRONTIERS====
In the mapping algorithm, we maintain a list of frontierpoints (2D points to represent position) for the sake of speed. That way, we only have to check the area, obtained during state [[#subsection SCANNING|SCANNING]], for new frontiers instead of going over the whole map. But because of that, we also need to remove any frontierpoints when they´re not frontiers anymore. DELETE_FRONTIERS is the state where every frontierpoint in the list is checked, to see whether it's still a frontier or not.

==== FIND_FRONTIER_NODES====
When all the outdated frontiers are deleted, we need to add the newly created frontiers. This happens in FIND_FRONTIER_NODES. Because the area that got updated in [[#subsection Scanning|SCANNING]] is the only area that got updated, we only have to search for new frontierpoints in there. Those frontierpoints will then be added to the list.

==== BUILD_FRONTIERGRID====
With the frontierpoint list we build a 2D grid representation of the map and fill it with frontiers on their respected positions (positions without frontiers are null pointers). This grid will be used in the next state to cluster them all together.

==== CLUSTER_FRONTIERS====
In this state, the frontiers will be clustered together using a [http://en.wikipedia.org/wiki/Disjoint-set_data_structure| Union-Find Algorithm]. With the list of frontierpoints, we can check the surrounding tiles of that position within the frontiergrid to find neighbouring frontiers. These neighbours are then clustered together. When the size of a cluster reaches a certain threshold, it is added to a list of clustered frontiers. The threshold has two advantages. One of them is that the drone will never visit frontiers that are too small to be of any use. The other advantage is to keep the amount of checks for redundant clusters in the list low. You see, when two clusters, that aren't the same, have been added to the list and later on these two clusters have formed 1 cluster, you'll have two of the same cluster in the list and one will have to be removed. With the threshold it will happen less often that two clusters inside the list will be unified.

=== Pick best frontier===
Having obtained several clusters of frontiers, we can now use these to deduct our next goal location. By normalizing the size of the frontiers and travelling distance towards them, we can pick the frontier with the optimum of small travelling distance and large size. However, before we can know the travelling distance towards a frontier we need to know the path towards it. For this we apply pathfinding in the FIND_PATHS state using the [http://www.policyalmanac.org/games/aStarTutorial.htm A* algorithm], we choose this algorithm mainly for it's simplicity. To make travelling along the path easier, we trim the found paths to a set of waypoints (not tile by tile). After all the paths have been found, the best frontier will be picked in the FIND_BEST_FRONTIER state.

=== Travel===
Now knowing where the drone should be headed to, we can start following the path we calculated earlier. Since the path exists out of waypoints, the easiest way of travelling is by turning towards the waypoint (TURN_TO_WAYPOINT), followed by moving forward until waypoint is reached (TRAVEL_TO_WAYPOINT). When the waypoint is reached you take the next waypoint and travel to that one, until there are no more waypoints left and you should have arrived at the goal.

== Integration with Paparazzi==

Since we had a hard time to get the AR.Drone2 flying, we ended up with too little time to integrate the mapping algorithm with Paparazzi. We did however work out how we wanted to integrate it. Our ideas will be explained in this section, just keep in mind that most of it is speculation.

For the mapping algorithm to work with Paparazzi, we need to be able to do a few things:
* Keep track of position and heading
* Pilot the AR.Drone2
* Read out the [[AR_Drone_2/Multidirectional_distance_measurement| distance sensor data]]
* Let Paparazzi call the algorithm in an iterative way

The first and third bullets are necessary to be able to know which maptiles to update. Keeping track of position and heading is also important for being able to calculate the next action to be taken by the drone (it's hard to figure out how to get somewhere if you don't know where you are). The second bullet is needed to be able to fly to where we want, how we want. The last thing we need is to execute the mapping algorithm, since Paparazzi is the main program, it will have to execute the mapping algorithm (and not the other way around).

=== Keep track of position and heading===
Paparazzi itself keeps track of a position and heading according to gps (this is all located in sw/airborne/state.h). Altough the heading is no problem (you can keep the map pointed north that way), it's hard to convert gps coördinates into map coördinates. But there's another solution to this: if we take the starting position as [0, 0] we can then update the position by integrating the velocity every iteration. Altough the outcome won't be 100% correct, the error will be kept to a minimum with a high enough update rate. In case velocity isn't updated fast enough we can also try using the acceleration from the accelerometers to update position.

=== Pilot the AR.Drone2===
Flying with Paparazzi currently works in two different ways: autonomous with a [[Flight_Plans| flight plan]] or on radio control with support of the autopilot until a certain degree. Flight plan based will mean that we'd have to be able to update the flight plan while flying from within the mapping algorithm. This is a hard thing to do since the flight plan is located in the ground station and the mapping algorithm will be working on the AR.Drone2 (the airborne part). Though it would be very nice and would also take away the first problem of keeping track of your own position (the flight plan can coöperate with relative x and y values).

The second option is a lot easier to implement, altough more prone to errors in the positioning. When using radio control, Paparazzi sends the radio control values from the ground station to the airborne. There, a struct gets filled with the values, which in turn gets handled by the autopilot. By using the right [[Rotorcraft_Configuration#Autopilot_modes| autopilot mode]] we can fly stable to the locations we want (AP_MODE_HOVER_Z_HOLD for yawing and AP_MODE_ATTITUDE_Z_HOLD for moving). Note however that by using the radio control struct you can only use 3 different autopilot modes (these are described in the [[Rotorcraft_Configuration| airframe]]).

=== Read out the distance sensor data===
To keep it simple, we wanted to directly access the data from the sensors. It is ofcourse possible to integrate the sensors with Paparazzi as well, but Paparazzi itself has no need for distance measurement.

=== Let Paparazzi call the algorithm in an iterative way===
To let Paparazzi use the mapping algorithm, we can implement mapping as a [[Subsystem| subsystem]]. That way mapping can be added optionally to the airframe (just like gps is optional). Since the mapping algorithm is to be called in an iterative way, we can tell the main control loop to call the algorithm and we're done.

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-31T12:16:39Z

Koudshoorn: /* Integration with paparazzi */

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum. Note however that, while true for some, not all states shift to the next after one iteration. In some cases, FIND_PATHS for example, it will take a few iterations before it will shift.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scanning===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===
In "Search for frontiers", we obviously search for frontiers. As the paper of Frontier-Based Exploration explains, we will use these frontiers to deduct our next goal to travel to. As can be seen in the statediagram, we created substates for "Search for frontiers" (DELETE_FRONTIERS, FIND_FRONTIER_NODES, BUILD_FRONTIERGRID and CLUSTER_FRONTIERS) for reasons explained earlier.

==== DELETE_FRONTIERS====
In the mapping algorithm, we maintain a list of frontierpoints (2D points to represent position) for the sake of speed. That way, we only have to check the area, obtained during state [[#subsection SCANNING|SCANNING]], for new frontiers instead of going over the whole map. But because of that, we also need to remove any frontierpoints when they´re not frontiers anymore. DELETE_FRONTIERS is the state where every frontierpoint in the list is checked, to see whether it's still a frontier or not.

==== FIND_FRONTIER_NODES====
When all the outdated frontiers are deleted, we need to add the newly created frontiers. This happens in FIND_FRONTIER_NODES. Because the area that got updated in [[#subsection Scanning|SCANNING]] is the only area that got updated, we only have to search for new frontierpoints in there. Those frontierpoints will then be added to the list.

==== BUILD_FRONTIERGRID====
With the frontierpoint list we build a 2D grid representation of the map and fill it with frontiers on their respected positions (positions without frontiers are null pointers). This grid will be used in the next state to cluster them all together.

==== CLUSTER_FRONTIERS====
In this state, the frontiers will be clustered together using a [http://en.wikipedia.org/wiki/Disjoint-set_data_structure| Union-Find Algorithm]. With the list of frontierpoints, we can check the surrounding tiles of that position within the frontiergrid to find neighbouring frontiers. These neighbours are then clustered together. When the size of a cluster reaches a certain threshold, it is added to a list of clustered frontiers. The threshold has two advantages. One of them is that the drone will never visit frontiers that are too small to be of any use. The other advantage is to keep the amount of checks for redundant clusters in the list low. You see, when two clusters, that aren't the same, have been added to the list and later on these two clusters have formed 1 cluster, you'll have two of the same cluster in the list and one will have to be removed. With the threshold it will happen less often that two clusters inside the list will be unified.

=== Pick best frontier===
Having obtained several clusters of frontiers, we can now use these to deduct our next goal location. By normalizing the size of the frontiers and travelling distance towards them, we can pick the frontier with the optimum of small travelling distance and large size. However, before we can know the travelling distance towards a frontier we need to know the path towards it. For this we apply pathfinding in the FIND_PATHS state using the [http://www.policyalmanac.org/games/aStarTutorial.htm| A* algorithm], we choose this algorithm mainly for it's simplicity. To make travelling along the path easier, we trim the found paths to a set of waypoints (not tile by tile). After all the paths have been found, the best frontier will be picked in the FIND_BEST_FRONTIER state.

=== Travel===
Now knowing where the drone should be headed to, we can start following the path we calculated earlier. Since the path exists out of waypoints, the easiest way of travelling is by turning towards the waypoint (TURN_TO_WAYPOINT), followed by moving forward until waypoint is reached (TRAVEL_TO_WAYPOINT). When the waypoint is reached you take the next waypoint and travel to that one, until there are no more waypoints left and you should have arrived at the goal.

== Integration with Paparazzi==

Since we had a hard time to get the AR.Drone2 flying, we ended up with too little time to integrate the mapping algorithm with Paparazzi. We did however work out how we wanted to integrate it. Our ideas will be explained in this section, just keep in mind that most of it is speculation.

For the mapping algorithm to work with Paparazzi, we need to be able to do a few things:
* Keep track of position and heading
* Pilot the AR.Drone2
* Read out the [[AR_Drone_2/Multidirectional_distance_measurement| distance sensor data]]
* Let Paparazzi call the algorithm in an iterative way

The first and third bullets are necessary to be able to know which maptiles to update. Keeping track of position and heading is also important for being able to calculate the next action to be taken by the drone (it's hard to figure out how to get somewhere if you don't know where you are). The second bullet is needed to be able to fly to where we want, how we want. The last thing we need is to execute the mapping algorithm, since Paparazzi is the main program, it will have to execute the mapping algorithm (and not the other way around).

=== Keep track of position and heading===
Paparazzi itself keeps track of a position and heading according to gps (this is all located in sw/airborne/state.h). Altough the heading is no problem (you can keep the map pointed north that way), it's hard to convert gps coördinates into map coördinates. But there's another solution to this: if we take the starting position as [0, 0] we can then update the position by integrating the velocity every iteration. Altough the outcome won't be 100% correct, the error will be kept to a minimum with a high enough update rate. In case velocity isn't updated fast enough we can also try using the acceleration from the accelerometers to update position.

=== Pilot the AR.Drone2===
Flying with Paparazzi currently works in two different ways: autonomous with a [[Flight_Plans| flight plan]] or on radio control with support of the autopilot until a certain degree. Flight plan based will mean that we'd have to be able to update the flight plan while flying from within the mapping algorithm. This is a hard thing to do since the flight plan is located in the ground station and the mapping algorithm will be working on the AR.Drone2 (the airborne part). Though it would be very nice and would also take away the first problem of keeping track of your own position (the flight plan can coöperate with relative x and y values).

The second option is a lot easier to implement, altough more prone to errors in the positioning. When using radio control, Paparazzi sends the radio control values from the ground station to the airborne. There, a struct gets filled with the values, which in turn gets handled by the autopilot. By using the right [[Rotorcraft_Configuration#Autopilot_modes| autopilot mode]] we can fly stable to the locations we want (AP_MODE_HOVER_Z_HOLD for yawing and AP_MODE_ATTITUDE_Z_HOLD for moving). Note however that by using the radio control struct you can only use 3 different autopilot modes (these are described in the [[Rotorcraft_Configuration| airframe]]).

=== Read out the distance sensor data===
To keep it simple, we wanted to directly access the data from the sensors. It is ofcourse possible to integrate the sensors with Paparazzi as well, but Paparazzi itself has no need for distance measurement.

=== Let Paparazzi call the algorithm in an iterative way===
To let Paparazzi use the mapping algorithm, we can implement mapping as a [[Subsystem| subsystem]]. That way mapping can be added optionally to the airframe (just like gps is optional). Since the mapping algorithm is to be called in an iterative way, we can tell the main control loop to call the algorithm and we're done.

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-31T09:35:52Z

Koudshoorn: /* Pick best frontier */

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum. Note however that, while true for some, not all states shift to the next after one iteration. In some cases, FIND_PATHS for example, it will take a few iterations before it will shift.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scanning===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===
In "Search for frontiers", we obviously search for frontiers. As the paper of Frontier-Based Exploration explains, we will use these frontiers to deduct our next goal to travel to. As can be seen in the statediagram, we created substates for "Search for frontiers" (DELETE_FRONTIERS, FIND_FRONTIER_NODES, BUILD_FRONTIERGRID and CLUSTER_FRONTIERS) for reasons explained earlier.

==== DELETE_FRONTIERS====
In the mapping algorithm, we maintain a list of frontierpoints (2D points to represent position) for the sake of speed. That way, we only have to check the area, obtained during state [[#subsection SCANNING|SCANNING]], for new frontiers instead of going over the whole map. But because of that, we also need to remove any frontierpoints when they´re not frontiers anymore. DELETE_FRONTIERS is the state where every frontierpoint in the list is checked, to see whether it's still a frontier or not.

==== FIND_FRONTIER_NODES====
When all the outdated frontiers are deleted, we need to add the newly created frontiers. This happens in FIND_FRONTIER_NODES. Because the area that got updated in [[#subsection Scanning|SCANNING]] is the only area that got updated, we only have to search for new frontierpoints in there. Those frontierpoints will then be added to the list.

==== BUILD_FRONTIERGRID====
With the frontierpoint list we build a 2D grid representation of the map and fill it with frontiers on their respected positions (positions without frontiers are null pointers). This grid will be used in the next state to cluster them all together.

==== CLUSTER_FRONTIERS====
In this state, the frontiers will be clustered together using a [http://en.wikipedia.org/wiki/Disjoint-set_data_structure| Union-Find Algorithm]. With the list of frontierpoints, we can check the surrounding tiles of that position within the frontiergrid to find neighbouring frontiers. These neighbours are then clustered together. When the size of a cluster reaches a certain threshold, it is added to a list of clustered frontiers. The threshold has two advantages. One of them is that the drone will never visit frontiers that are too small to be of any use. The other advantage is to keep the amount of checks for redundant clusters in the list low. You see, when two clusters, that aren't the same, have been added to the list and later on these two clusters have formed 1 cluster, you'll have two of the same cluster in the list and one will have to be removed. With the threshold it will happen less often that two clusters inside the list will be unified.

=== Pick best frontier===
Having obtained several clusters of frontiers, we can now use these to deduct our next goal location. By normalizing the size of the frontiers and travelling distance towards them, we can pick the frontier with the optimum of small travelling distance and large size. However, before we can know the travelling distance towards a frontier we need to know the path towards it. For this we apply pathfinding in the FIND_PATHS state using the [http://www.policyalmanac.org/games/aStarTutorial.htm| A* algorithm], we choose this algorithm mainly for it's simplicity. To make travelling along the path easier, we trim the found paths to a set of waypoints (not tile by tile). After all the paths have been found, the best frontier will be picked in the FIND_BEST_FRONTIER state.

=== Travel===
Now knowing where the drone should be headed to, we can start following the path we calculated earlier. Since the path exists out of waypoints, the easiest way of travelling is by turning towards the waypoint (TURN_TO_WAYPOINT), followed by moving forward until waypoint is reached (TRAVEL_TO_WAYPOINT). When the waypoint is reached you take the next waypoint and travel to that one, until there are no more waypoints left and you should have arrived at the goal.

== Integration with paparazzi==

Since we had a hard time to get the AR.Drone2 flying, we ended up with too little time to integrate the mapping algorithm with paparazzi. We did however work out how we wanted to integrate it. Our ideas will be explained in this section.

For the mapping algorithm to work with paparazzi, we need to be able to do a few things:
* Keep track of position and heading
* Directly control the AR.Drone2 while keeping the controller part of paparazzi active
* Read out the [[AR_Drone_2/Multidirectional_distance_measurement| distance sensor data]]

The first and third are necessary to be able to know what tiles to update of the map and what tile not. The second is to be able to fly around to where we want, how we want.

=== Keep track of position and heading===

=== Directly control the AR.Drone2===

=== Read out the distance sensor data===

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-31T09:34:07Z

Koudshoorn: /* Integration with paparazzi */ linked to the page AR_Drone_2/Multidirectional_distance_measurement

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum. Note however that, while true for some, not all states shift to the next after one iteration. In some cases, FIND_PATHS for example, it will take a few iterations before it will shift.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scanning===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===
In "Search for frontiers", we obviously search for frontiers. As the paper of Frontier-Based Exploration explains, we will use these frontiers to deduct our next goal to travel to. As can be seen in the statediagram, we created substates for "Search for frontiers" (DELETE_FRONTIERS, FIND_FRONTIER_NODES, BUILD_FRONTIERGRID and CLUSTER_FRONTIERS) for reasons explained earlier.

==== DELETE_FRONTIERS====
In the mapping algorithm, we maintain a list of frontierpoints (2D points to represent position) for the sake of speed. That way, we only have to check the area, obtained during state [[#subsection SCANNING|SCANNING]], for new frontiers instead of going over the whole map. But because of that, we also need to remove any frontierpoints when they´re not frontiers anymore. DELETE_FRONTIERS is the state where every frontierpoint in the list is checked, to see whether it's still a frontier or not.

==== FIND_FRONTIER_NODES====
When all the outdated frontiers are deleted, we need to add the newly created frontiers. This happens in FIND_FRONTIER_NODES. Because the area that got updated in [[#subsection Scanning|SCANNING]] is the only area that got updated, we only have to search for new frontierpoints in there. Those frontierpoints will then be added to the list.

==== BUILD_FRONTIERGRID====
With the frontierpoint list we build a 2D grid representation of the map and fill it with frontiers on their respected positions (positions without frontiers are null pointers). This grid will be used in the next state to cluster them all together.

==== CLUSTER_FRONTIERS====
In this state, the frontiers will be clustered together using a [http://en.wikipedia.org/wiki/Disjoint-set_data_structure| Union-Find Algorithm]. With the list of frontierpoints, we can check the surrounding tiles of that position within the frontiergrid to find neighbouring frontiers. These neighbours are then clustered together. When the size of a cluster reaches a certain threshold, it is added to a list of clustered frontiers. The threshold has two advantages. One of them is that the drone will never visit frontiers that are too small to be of any use. The other advantage is to keep the amount of checks for redundant clusters in the list low. You see, when two clusters, that aren't the same, have been added to the list and later on these two clusters have formed 1 cluster, you'll have two of the same cluster in the list and one will have to be removed. With the threshold it will happen less often that two clusters inside the list will be unified.

=== Pick best frontier===
Having obtained several clusters of frontiers, we can now use these to deduct our next goal location. By normalizing the size of the frontiers and travelling distance towards them, we can pick the frontier with the optimum of small travelling distance and large size. However, before we can know the travelling distance towards a frontier we need to know the path towards it. For this we apply pathfinding in the FIND_PATHS state using the [http://www.policyalmanac.org/games/aStarTutorial.htm| A* algorithm]. To make travelling along the path easier, we trim the found paths to a set of waypoints (not tile by tile). After all the paths have been found, the best frontier will be picked in the FIND_BEST_FRONTIER state.

=== Travel===
Now knowing where the drone should be headed to, we can start following the path we calculated earlier. Since the path exists out of waypoints, the easiest way of travelling is by turning towards the waypoint (TURN_TO_WAYPOINT), followed by moving forward until waypoint is reached (TRAVEL_TO_WAYPOINT). When the waypoint is reached you take the next waypoint and travel to that one, until there are no more waypoints left and you should have arrived at the goal.

== Integration with paparazzi==

Since we had a hard time to get the AR.Drone2 flying, we ended up with too little time to integrate the mapping algorithm with paparazzi. We did however work out how we wanted to integrate it. Our ideas will be explained in this section.

For the mapping algorithm to work with paparazzi, we need to be able to do a few things:
* Keep track of position and heading
* Directly control the AR.Drone2 while keeping the controller part of paparazzi active
* Read out the [[AR_Drone_2/Multidirectional_distance_measurement| distance sensor data]]

The first and third are necessary to be able to know what tiles to update of the map and what tile not. The second is to be able to fly around to where we want, how we want.

=== Keep track of position and heading===

=== Directly control the AR.Drone2===

=== Read out the distance sensor data===

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-30T20:55:38Z

Koudshoorn: /* Integration with paparazzi */

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum. Note however that, while true for some, not all states shift to the next after one iteration. In some cases, FIND_PATHS for example, it will take a few iterations before it will shift.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scanning===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===
In "Search for frontiers", we obviously search for frontiers. As the paper of Frontier-Based Exploration explains, we will use these frontiers to deduct our next goal to travel to. As can be seen in the statediagram, we created substates for "Search for frontiers" (DELETE_FRONTIERS, FIND_FRONTIER_NODES, BUILD_FRONTIERGRID and CLUSTER_FRONTIERS) for reasons explained earlier.

==== DELETE_FRONTIERS====
In the mapping algorithm, we maintain a list of frontierpoints (2D points to represent position) for the sake of speed. That way, we only have to check the area, obtained during state [[#subsection SCANNING|SCANNING]], for new frontiers instead of going over the whole map. But because of that, we also need to remove any frontierpoints when they´re not frontiers anymore. DELETE_FRONTIERS is the state where every frontierpoint in the list is checked, to see whether it's still a frontier or not.

==== FIND_FRONTIER_NODES====
When all the outdated frontiers are deleted, we need to add the newly created frontiers. This happens in FIND_FRONTIER_NODES. Because the area that got updated in [[#subsection Scanning|SCANNING]] is the only area that got updated, we only have to search for new frontierpoints in there. Those frontierpoints will then be added to the list.

==== BUILD_FRONTIERGRID====
With the frontierpoint list we build a 2D grid representation of the map and fill it with frontiers on their respected positions (positions without frontiers are null pointers). This grid will be used in the next state to cluster them all together.

==== CLUSTER_FRONTIERS====
In this state, the frontiers will be clustered together using a [http://en.wikipedia.org/wiki/Disjoint-set_data_structure| Union-Find Algorithm]. With the list of frontierpoints, we can check the surrounding tiles of that position within the frontiergrid to find neighbouring frontiers. These neighbours are then clustered together. When the size of a cluster reaches a certain threshold, it is added to a list of clustered frontiers. The threshold has two advantages. One of them is that the drone will never visit frontiers that are too small to be of any use. The other advantage is to keep the amount of checks for redundant clusters in the list low. You see, when two clusters, that aren't the same, have been added to the list and later on these two clusters have formed 1 cluster, you'll have two of the same cluster in the list and one will have to be removed. With the threshold it will happen less often that two clusters inside the list will be unified.

=== Pick best frontier===
Having obtained several clusters of frontiers, we can now use these to deduct our next goal location. By normalizing the size of the frontiers and travelling distance towards them, we can pick the frontier with the optimum of small travelling distance and large size. However, before we can know the travelling distance towards a frontier we need to know the path towards it. For this we apply pathfinding in the FIND_PATHS state using the [http://www.policyalmanac.org/games/aStarTutorial.htm| A* algorithm]. To make travelling along the path easier, we trim the found paths to a set of waypoints (not tile by tile). After all the paths have been found, the best frontier will be picked in the FIND_BEST_FRONTIER state.

=== Travel===
Now knowing where the drone should be headed to, we can start following the path we calculated earlier. Since the path exists out of waypoints, the easiest way of travelling is by turning towards the waypoint (TURN_TO_WAYPOINT), followed by moving forward until waypoint is reached (TRAVEL_TO_WAYPOINT). When the waypoint is reached you take the next waypoint and travel to that one, until there are no more waypoints left and you should have arrived at the goal.

== Integration with paparazzi==

Since we had a hard time to get the AR.Drone2 flying, we ended up with too little time to integrate the mapping algorithm with paparazzi. We did however work out how we wanted to integrate it. Our ideas will be explained in this section.

For the mapping algorithm to work with paparazzi, we need to be able to do a few things:
* Keep track of position and heading
* Directly control the AR.Drone2 while keeping the controller part of paparazzi active
* Read out the distance sensor data

The first and third are necessary to be able to know what tiles to update of the map and what tile not. The second is to be able to fly around to where we want, how we want.

=== Keep track of position and heading===

=== Directly control the AR.Drone2===

=== Read out the distance sensor data===

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-30T13:09:25Z

Koudshoorn: /* CLUSTER_FRONTIERS */ fixed a link

AR Drone 2/Mapping

2013-01-30T13:08:57Z

Koudshoorn: /* Pick best frontier */ link to A* now works

AR Drone 2/Mapping

2013-01-30T13:07:28Z

Koudshoorn: /* Pick best frontier */ Link was not working properly

AR Drone 2/Mapping

2013-01-30T13:06:28Z

Koudshoorn: Explained all the states of the state diagram

AR Drone 2/Mapping

2013-01-29T21:50:20Z

Koudshoorn: made top msg colored

File:Statediagram mapping.png

2013-01-29T21:44:29Z

Koudshoorn: uploaded a new version of "File:Statediagram mapping.png": the png was incomplete, this is the whole statediagram

Statediagram of mapping algorithm with the AR.Drone2

File:Statediagram mapping.png

2013-01-29T21:43:27Z

Koudshoorn: Statediagram of mapping algorithm with the AR.Drone2

Statediagram of mapping algorithm with the AR.Drone2

AR Drone 2/Mapping

2013-01-29T21:42:25Z

Koudshoorn: updated the used datastructure and edited a few states of the algorithm

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

'''Mapping has not been integrated with paparazzi due to lack of time. Yet, all components have been tested and are working properly.'''

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position and heading, store a map efficiëntly and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to use a map and still be able to go through all the control loops of paparazzi, the fourth is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, at first we believed [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure using an [http://en.wikipedia.org/wiki/Octree OcTree] to store a 3D map that is updateable, flexible and compact. However, since we want to fly in a 2D manner, it's not very usefull. You'd have to retreive a 2D slice of the map at a certain hight (for which there is no build-in function) every iteration which would take a lot of unnecessary time.

Since we liked the flexibility of the Octomap, we created our own 2D version of it: a quadmap so to speak. Instead of an octree, it uses a quadtree to store information about the map. This way we represent the map as a square instead of a cube, like the Octomap does. The rest is pretty much the same: we keep a record of all the occupied, free and unknown cells and we can update the map by casting rays from a startingpoint towards an endpoint (with endpoint being occupied or not).

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects, it is pretty much impossible to create a 3D map. Flying on the same hight all the time, will make a 2D map sufficient for the job. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Pick best frontier
* Travel

We can use these steps to create a statemachine. Since paparazzi needs to go through its control loops to keep the drone in the air (and because it's a single threaded system), we need to execute the mapping algorithm in smaller steps and thus keep track of what state we are.

[[Image:statediagram_mapping.png|thumb|center|700px|Statediagram of Frontier-Based Exploration with the AR.Drone2]]

As you can see, some of the steps of Frontier-Based Exploration have been split up into smaller substates. This way we can see more easily what the program is supposed to do while keeping the operation time per iteration to a minimum.

=== Init===
Inside the mapping algorithm, we not only keep track of the state, we also keep track of other things like indexes, iterator positions etc. In the init state, all these fields are set to their respective starting values, after which the algorithm enters the next state.

=== Scan===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===

=== Find paths===

=== Pick best frontier===

=== Travel===

== Avoiding objects==

To be able to avoid object, you need to know your surroundings. That's why the object avoidance function is closely related to mapping. Therefore it will probably be developed simultaneously.

[[Category: AR Drone 2]]

AR Drone 2/Mapping

2013-01-11T15:04:42Z

Koudshoorn:

<categorytree style="float:right; clear:right; margin-left:1ex; border: 1px solid gray; padding: 0.7ex;" mode=pages>AR Drone 2</categorytree>

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position, yaw and pitch and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, we believe [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure to store a 3D map that is updateable, flexible and compact. One minor setback was that it was implemented in C++ instead of C. At first this seemed like a problem, but after some research it was concluded that this should be no problem. C code can easily use C++ code if a wrapper is used to link C++ methods to C ([http://stackoverflow.com/questions/199418/using-c-library-in-c-code link]). This was also tested with a small program: a C program that increments a number using a method of a C++ program. With this it was confirmed that it really shouldn't be a problem to use Octomap within paparazzi.

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects it is better to approach the exploring as a 2D problem and solve this at different heights and thus still creating a 3D map. We're using [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration] for this purpose. With this approach you start with a map filled with unknown cells. Then, by scanning the surroundings of the robot, you fill in the unknown cells: occupied or unoccupied. Next, you search all the frontiers (the border between unoccupied and unknown cells) and pick the frontier with the optimum of being the largest and with the least travel distance. Subsequently you take that frontier as your next destination and upon arival you scan again. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect for example) and therefore not suited for our AR.Drone2.

As explained, Frontier-Based Exploration consists out of a few steps that will be repeated until the whole area is explored:
* Scan
* Search for frontiers
* Find paths
* Pick best frontier
* Travel
The reason find paths also is in that list, is because it is neccessary for computing the travel distance (pick best frontier) and for finding a path (travel). Since this would mean that we'd have to execute the same action twice it's better to put it as a single step. Another solution would be to estimate travel distance instead of computing a path for all the frontiers, but we couldn't find a reliable way to do so (as the bird flies isn't a good method considering there could be frontiers behind a wall).

=== Scan===
Some robots can scan entire areas while moving with wide range sensors. Our Ar.Drone2 is limited to point measurement sensors and therefore has to have an explicit step "Scan". Seeing as our drone will have 4 horizontally placed sensors in a + sign, a 360 degrees scan can be made by making a 45 degrees turn. By keeping track of our rotation and position, with the use of internal sensors, we can deduct where the scanned points are in our selfmade map.

=== Search for frontiers===

=== Find paths===

=== Pick best frontier===

=== Travel===

== Avoiding objects==

To be able to avoid object, you need to know your surroundings. That's why the object avoidance function is closely related to mapping. Therefore it will probably be developed simultaneously.

[[Category: AR Drone 2]]

Talk:TU Delft: AR Drone 2 - News

2012-12-14T14:03:14Z

Koudshoorn:

== TU Delft Robotics minor: AR Drone 2 project - News ==
Here we will post breakthroughs, summaries of a page update and small facts that might be nice to know.

=== GPS on the AR Drone 2 [14-12-2012]===
Altough we still need to test the protocol itself, we've managed to compile, with paparazzi, all the necessary files to read the SiRF protocol. Remaining work: let the GPS output the SiRF protocol on the AR Drone and test it.

=== General progress update [12-12-2012] ===
It has been a while sinds we posted anything here. It is not that we haven't been busy, it's quite the opposite. We have managed read the navigation data structure provided by the navigation board of the AR Drone 2. This contains the gyro-meter, accelerometer, magnetometer and barometer data we needed. With this we managed to implement the IMU and feen the gyro-, accelero- and magnetometer data to it. This now readies the data for the AHRS. The barometer sensor is now implemented as well, sending the absolute pressure value on to the AHRS and INS.

Today we took another step forward. Pararazzi can now opperate the actuators, with the use of motor mixing, and set the thrust of each rotor according to the pitch, yaw, roll and thrust values provided to it.

=== Build a GPS driver for the AR Drone [30-11-2012] ===
Today we finally managed to build and compile a driver for the AR Drone 2. As you may know, we want to connect a GPS sensor to the Drone and since there is not an USB-serial driver available on the Drone we have to make it by ourself. Today we finished the job and succesfully received GPS data on the AR Drone. Now we know how to make a driver for the Drone it will be easier to write other drivers if necessary.

=== Compiling the AP breakthrough! [26-11-2012] ===
Today we (finally) managed to compile the AR Drone 2 auto pilot without errors! Not all subsystems have been implemented yet so we can't test it on the drone for a while but at least we managed fix the linkage errors we were getting constantly.

[[Category: TU Delft - Automonous Quadrotor]]

AR Drone 2/Mapping

2012-12-05T12:46:50Z

Koudshoorn: typo corrections

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position, yaw and pitch and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, we believe [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper]) to be a good solution. It's a datastructure to store a 3D map that is updateable, flexible and compact. One minor setback was that it was implemented in C++ instead of C. At first this seemed like a problem, but after some research it was concluded that this should be no problem. C code can easily use C++ code if a wrapper is used to link C++ methods to C ([http://stackoverflow.com/questions/199418/using-c-library-in-c-code link]). This was also tested with a smal l program: a C program that increments a number using a method of a C++ program. With this it was confirmed that it really shouldn't be a problem to use Octomap within paparazzi.

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects it might be best to approach the exploring as a 2D problem and solve this at different heights. A popular approach is [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration]. A summary: you store free and occupied cells, other cells are unknown, you try to find the largest frontier (the border between free and unknown cells) with the shortest travel distance from your current position. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf paper]). But this is based on a wide range sensor (kinect) and therefore less suitable for the AR.Drone2.

== Avoiding objects==

To be able to avoid object, you need to know your surroundings. That's why the object avoidance function is closely related to mapping. Therefore it will probably be developed simultaneously.

AR Drone 2/Mapping

2012-12-05T12:45:44Z

Koudshoorn: new page about the mapping approach with the AR.Drone2

One of the functions that the AR.Drone2 should have is to be able to map a certain area (starting off with a room). In order to do this the AR.Drone2 needs to be able to: measure distance to objects, keep track of it's own position, yaw and pitch and avoid crashing into objects. The first two things are needed to create a map from measurements during flight, the third is to maintain flight. It would also be usefull to make an algorithm for the AR.Drone2 to explore the area and store the map in the most efficient way.

== Storing the map==

For storing the map, we believe [http://octomap.sf.net/ Octomap] ([http://ais.informatik.uni-freiburg.de/publications/papers/wurm10octomap.pdf paper)] to be a good solution. It's a datastructure to store a 3D map that is updateable, flexible and compact. One minor setback was that it was implemented in C++ instead of C. At first this seemed like a problem, but after some research it was concluded that this should be no problem. C code can easily use C++ code if a wrapper is used to link C++ methods to C ([http://stackoverflow.com/questions/199418/using-c-library-in-c-code link]). This was also tested with a smal l program: a C program that increments a number using a method of a C++ program. With this it was confirmed that it really shouldn't be a problem to use Octomap within paparazzi.

== Exploring the area==

Since the AR.Drone2 will only have point measurement sensors to measure distance to objects it might be best to approach the exploring as a 2D problem and solve this at different heights. A popular approach is [http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=613851 Frontier-Based Exploration]. A summary: you store free and occupied cells, other cells are unknown, you try to find the largest frontier (the border between free and unknown cells) with the shortest travel distance from your current position. There are also variations for 3D ([http://www.informatik.uni-freiburg.de/~ki/papers/dornhege_etal_ssrr11.pdf link]). But this was based on a wide range sensor (kinect) and therefore less suitable for the AR.Drone2.

== Avoiding objects==

To be able to avoid object, you need to know your surroundings. That's why the object avoidance function is closely related to mapping. Therefore it will probably be developed simultaneously.

File:FlightplanSeq.png

2012-11-22T13:25:26Z

Koudshoorn: Sequence diagram of the actions of airborne when a datalink message is received

Sequence diagram of the actions of airborne when a datalink message is received

File:DataAcquisitionSeq.png

2012-11-22T10:47:58Z

Koudshoorn: Sequence diagram of how the data acquisition works inside the Airborne

Sequence diagram of how the data acquisition works inside the Airborne

TU Delft - Search and Rescue with AR Drone 2

2012-11-22T10:28:13Z

Koudshoorn: new paragraph about the architecture of paparazzi

[[Image:2012-10-03 12.40.46.jpg|thumb|right|400px|TU Delft Minor Robotics: Quadrotor Group 1]]__TOC__

== Introduction==
The highly maneuverability and stability of a quadrotor inspired us to make an autonomous search and rescue vehicle. For this the AR Drone 2 was used. The goal of this project is thus to further develop the AR Drone2 in a robot capable of performing an autonomous search mission. All the steps taken are described on this page.

In order to achieve this goal we decided to work with raw data. The data was obtained by creating drivers capable of extracting and interpreting sensor data from the AR Drone2. In order to get the quadrotor flying paparazzi was used. For simulation JSBSim was used, which was then visualized using Flight Gear. The final section of the page will concern with the subject of making the quadrotor autonomous.

First some general information about the project in given in chapter2. How all the data was obtained is presented in chapter 3. In chapter 4 it is described how to get paparazzi working on the drone. Next in chapter 5 we present how simulation was done and finally the proces of making the drone autonomous is described in chapter 6.

== General project information==

This project was develloped using Linux, more specific Ubunutu. So except for the simulation part all steps are explained for this operating system. But it's a free operating system, so if you want to retrace our steps you can simply download it and access it from your current operating system using virtual box. A small tutorial can be found [[ Virtualbox 4.1.22 for Windows Hosts & Ubuntu 12.04 LTS| here]].

For this project we used a repository, Github. This allows multiple people to work on the same project at the same time without getting in eachother's way. So all files assosiated with our project assosiated can be accesed using Github. For beginners, you can read the manual to setup Github for Ubuntu:

* [[Github manual for Ubuntu]]

Our git repository is at [https://github.com/RoboticaTUDelft/paparazzi Github RoboticaTUDelft] in the "minor1" branch. You can change to this branch bij executing the following command "git checkout minor1". The related project, [[TU Delft - Lasergame with Autonomous AR Drone]], can be found under the minor2 branch.

== Data acquisition==

So as explained in the introduction, firstly the raw data had to be extracted from the AR Drone2. So this data could be fed to paparazzi. So we started with trying to connect and pass files to the AR Drone2, which is desribed in the connecing section. The exact details of the extraction of the data is described in the NAV board section. Next the data from paparazzi had to be passed on to our actuators correctly. This is described in the motor driver section. To be able to perform better in our autonomous mission a GPS was added to the AR Drone2. The details of this are described in the GPS section.

=== Connecting===

For our project the operating system Ubuntu was used, so all the steps described here are for an Ubuntu operating system.

The connecting was done using Telnet.

*[[Telnet AR.Drone2]]

The actual transferring of files is done using a File Transfer Protocol (FTP).

*[[FTP AR.Drone2]]

=== NAV board===

Throughout this project, on several different occasions, it will be apparent that the AR Drone2 was never really meant to be used as we inteded to. In addition to this, Parrot SA has a vested interest in maintaining the exclusivity of its product, rightfully so. So with what little information is publicly available, reverse engineering was a lot harder than we first thought when we began the project. However the AR.Drone2 looks a lot like the AR.Drone1, its predecessor, for which a lot of reverse engineering has allready been done by other people. Ofcourse there are differences and this is where we started reverse engineering the bits and pieces for the NAV board.

*[[NAV board AR.Drone2]]

=== Motor driver===

Using a similar technique as for the NAV board, the motor drivers were obtained.

*[[Motor driver AR.Drone2]]

=== GPS===
The GPS we use with the AR.Drone2 is the [[GPS_specification | BU-353 GPS]]. To make good use of this GPS we had to create our own driver for it.

*[[GPS Driver AR.Drone2]]

== Paparazzi==

Now the necessary raw data can be obtained and the actuators can be controlled, it's time to focus on paparazzi. This section consists of two parts. The first part deals with integrating the AR.Drone2 into the paparazzi libraries, the second part deals with compiling paparazzi for the drone. More inforamtion on how to install paparazzi can be found [[installation|here]].

=== Using Paparazzi===

This parts describes the process of adapting the paparazzi libraries to make them compatible with the AR.Drone’s hard en software.

*[[Integrating the AR Drone 2 with Paparazzi]]

=== Compiling===

Finally paparazzi can be compiled and be put on the AR.Drone 2. This is describes in this section.

*[[compiling paparazzi for the AR.Drone2]]

=== Paparazzi architecture===

In the near future, we'd like to add features like object avoidance. Since this isn't yet implemented in paparazzi we have to start from scratch. Therefore we tried to understand the architecture a little better than is explained on this wiki at the moment.

*[[Insight of paparazzi architecture]]

== Simulation==

Now paparazzi was compiled and our drone was ready to go, simulation could begin. The simulation was done in JSBSim, which was then visualized using flightgear. This are also the 2 parts in this section. More information on how to install JSBSim see [[JSBSim]]. For more information on how to install Flightgear and link it to Paparazzi see [[Simulation#View_the_simulation_in_Flight_Gear]].

=== JSBSim===
'''Creating A Flight Dynamics Model'''

In order to control the otherwise highly active dynamic nature of the quadrotor, an autopilot system will be necessary because the reaction time of the pilot will, otherwise, not be sufficient to maintain stable flight. The autopilot system will be created in Paparazzi for which a prerequisite is a proper flight dynamics model (FDM). To create the FDM the team will use [http://jsbsim.sourceforge.net/ JSBSIM], an open-source, platform-independent, flight-dynamics and control software library. Installing JSBSIM is covered for several different operating systems on the Paparazzi Wiki, [[JSBSim|Installing JSBSIM]].

Once JSBSim had been installed, the actual work could begin. Flight Dynamics and the creation of an individual model for an aircraft is essentially a case study regarding its control, stability, and performance. To model those three dynamics aspects is essentially what JSBSIM was used for but before work could begin, JSBSIM would first need to be fed a configuration file. The configuration file is written in a .xml format and, naturally, is unique to each aircraft. Although not applicable to this project, it is worth mentioning in the event that the reader of this page is not following the instruction exactly and is using it more for reference, there is a tool on the JSBSIM page called [http://jsbsim.sourceforge.net/aeromatic2.html Aeromatic], which is a capable .xml configuration file generator. For those following this project closely, a link will follow for creating a suitable JSBSIM .xml configuration file for the AR.Drone2.

*[[Creating a JSBSIM .xml Configuration File for AR.Drone2]]

=== Flightgear===
Without actually using the drone itself, and only the previously developed JSBSIM configuration file, a simulated drone can be flown in Flightgear. To do so, a CAD model of the drone is implemented to give a visualization to the simulated model. Flightgear is the preeminent open-source flight simulator backed by a large community of active users. Implementation for this particular project can be read about on the [[AR.Drone 2 - Flightgear]] wikipage. More on the software can be found on the [http://www.flightgear.org/ Flightgear website].

== Autonomous==

TU Delft - Search and Rescue with AR Drone 2

2012-11-15T11:00:54Z

Koudshoorn:

__TOC__

== Introduction==
The highly maneuverability and stability of a quadrotor inspired us to make an autonomous search and rescue vehicle. For this the AR Drone 2 was used. The goal of this project is thus to further develop the AR Drone2 in a robot capable of performing an autonomous search mission. All the steps taken are described on this page.

In order to achieve this goal we decided to work with raw data. The data was obtained by creating drivers capable of extracting and interpreting sensor data from the AR Drone2. In order to get the quadrotor flying paparazzi was used. For simulation JSBSim was used, which was then visualized using Flight Gear. The final section of the page will concern with the subject of making the quadrotor autonomous.

How all the data was obtained is presented in chapter 2. In chapter 3 it is described how to get paparazzi working on the drone. Next in chapter 4 we present how simulation was done and finally the proces of making the drone autonomous is described in chapter 5.

== Data acquisition==

So as explained in the introduction, firstly the raw data had to be extracted from the AR Drone2. So this data could be fed to paparazzi. So we started with trying to connect and to pass files to the AR Drone2, which is desribed in the connecing section. The exact details of the extraction of the data is described in the NAV board section. Next the data from paparazzi had to be passed on to our actuators correctly. This is described in the drivers section. To be able to perform better in our autonomous mission a GPS was added to the AR Drone2. The details of this are described in the GPS section.

=== Conecting===

For our project the operating system Ubuntu was used, so all the steps described here are for an Ubuntu operating system. But it's a free operating system, so if you want to retrace our steps you can simply download it and for example access it from your current operating system using virtual box.

==== Telnet====

The actual connecting was done using Telnet. After installing Telnet a connecting can be established in the following manner: (dino of iemand andners, kan je dit aanvullen?)

==== FTP====

For transferring files the File Transfer Protocol (FTP) used in this project, was FileZille. Files can be transferred to the AR Drone2 as described next: (dino of iemand andners, kan je dit aanvullen?)

=== NAV board===

Throughout this project, on several different occasions, it will be apparent that the AR Drone2 was never rally meant to be used as we inteded to. In addition to this, Parrot SA has a vested interest in maintaining the exclusivity of its product, rightfully so. So with what little information is publicly available, reverse engineering was a lot harder than we first thought when we began the project. However AR.Drone2 looks a lot like the AR.Drone1, its predecessor, for which a lot of reverse engineering has allready been done by other people. Ofcourse there are differences and this is where we started reverse engineering the bits and pieces for the NAV board.

*[[NAV board AR Drone 2]]

=== Motor driver===

Starting off with the project we first tried searching what had allready been done for AR.Drone1 and we found a very good source[http://blog.perquin.com/blog/2011/06/] explaining
how to control the motors. We soon noticed that the device name is different on the AR.Drone2. We found out by using dmesg and trace functions that the devices are as follows:

*/dev/ttyO0 : motor

After finding out what the correct device was, we could simply change the device name in the driver created by hugo[http://blog.perquin.com/blog/ar-drone-program-elf-replacement/].
This gave us a working motor driver.

*[[Motor driver AR.Drone2]]

=== GPS===
The GPS we use with the AR.Drone2 is the [[GPS_specification | BU-353 GPS]]. To make good use of this GPS we had to create our own driver for it. That way we could extract the received data from the GPS and output it in our preferred format.
To create our own driver, we first needed to know how to retrieve the data from the GPS. We downloaded the [http://www.usglobalsat.com/s-122-bu-353-support.aspx Linux USB Driver] and learned from the readme that you could use the GPS with the following commando:
su root

stty -F /dev/ttyUSB0 ispeed 4800 && cat < /dev/ttyUSB0
With this we found out that we needed to create a program that opens the /dev/ttyUSB0 device and reads it at a baud rate of 4800. We had to pick a datatype since working with several datatypes at the same time would be confusing. Therefore we had to extract the right datatype string from the data strings output by the GPS and format that string into useful information.

Later, when we were trying to integrate the GPS with paparazzi, we found out that there was already a parser for the protocol of the GPS: the NMEA. This parser (altough stated to be incomplete) provides most of the necassary data we wanted to use. We therefore ceased working on our own parser.

== Paparazzi on Drone==

Now the necessary raw data ould be obtained and the actuators could be controlled, it's time to integrate

=== Integrating drone in Paparazzi===

""Paparazzi Autopilot System""
Throughout this project, on several different occasions, it will be apparent that the AR.Drone was never meant to be used in this fashion. In addition to this, Parrot SA has a vested interest in maintaining the exclusivity of its product, rightfully so. With what little information is publicly available it is the team's objective to load the open-source autopilot system, Paparazzi onto an otherwise copyright protected piece of hardware. In order to do so a few steps had to be taken to get Paparazzi onto the AR.Drone 2.

*[[Reverse engineering the AR.Drone2]]

=== Compiling===

== Simulation==

=== JSBSim===
'''Creating A Flight Dynamics Model'''

In order to control the otherwise highly active dynamic nature of the quadrotor, an autopilot system will be necessary because the reaction time of the pilot will, otherwise, not be sufficient to maintain stable flight. The autopilot system will be created in Paparazzi for which a prerequisite is a proper flight dynamics model (FDM). To create the FDM the team will use [http://jsbsim.sourceforge.net/ JSBSIM], an open-source, platform-independent, flight-dynamics and control software library. Installing JSBSIM is covered for several different operating systems on the Paparazzi Wiki, [[JSBSim|Installing JSBSIM]].

Once JSBSim had been installed, the actual work could begin. Flight Dynamics and the creation of an individual model for an aircraft is essentially a case study regarding its control, stability, and performance. To model those three dynamics aspects is essentially what JSBSIM was used for but before work could begin, JSBSIM would first need to be fed a configuration file. The configuration file is written in a .xml format and, naturally, is unique to each aircraft. Although not applicable to this project, it is worth mentioning in the event that the reader of this page is not following the instruction exactly and is using it more for reference, there is a tool on the JSBSIM page called [http://jsbsim.sourceforge.net/aeromatic2.html Aeromatic], which is a capable .xml configuration file generator. For those following this project closely, a link will follow for creating a suitable JSBSIM .xml configuration file for the AR.Drone2.

*[[Creating a JSBSIM .xml Configuration File for AR.Drone2]]

=== Flightgear===
Without actually using the drone itself, and only the previously developed JSBSIM configuration file, a simulated drone can be flown in Flightgear. To do so, a CAD model of the drone is implemented to give a visualization to the simulated model. Flightgear is the preeminent open-source flight simulator backed by a large community of active users. More on the software can be found on the [http://www.flightgear.org/ Flightgear website].

== Autonomous==

== Creating an airframe ==
From the Paparazzi GCS we need to add an aircraft and attach an [[airframe]], flightplan, settings, radio and telemetry files.

TU Delft - Search and Rescue with AR Drone 2

2012-11-14T10:13:20Z

Koudshoorn:

TU Delft - Search and Rescue with AR Drone 2

2012-11-14T10:03:55Z

Koudshoorn:

TU Delft - Search and Rescue with AR Drone 2

2012-11-14T09:59:59Z

Koudshoorn:

TU Delft - Search and Rescue with AR Drone 2

2012-11-14T09:36:41Z

Koudshoorn: