In this tutorial we will walk through the process of using stereo camera and octomap for environment perception and A* for path finding in an unknown environment.
Last updated
Give us a Star on Github if you find this tutorial useful.
Help us to understand what you want to learn in the upcoming tutorials by fill out this three-question survey.
---------------------
We suggest developers to install GAAS mirror to use the project:
Should you have any questions or suggestions to the tutorials or GAAS itself, let us know and join our slack
In the previous tutorials, we have talked about how to control your drone with python and enable your drone to fly in GPS denied environments, which is a big step towards autonomous drone. Now, we will talk about how to enable your drone to fly in an unknown environment from point A to point B, with the help of a pair of stereo camera only, in Gazebo simulator. This tutorial can be organized as follow:
Obstacle distance estimation with Stereo Camera;
Octomap as a way to represent the environment;
A* path finding in 3D space;
Simple path pruning;
Again, I have to stress that you will need some extra work before going to the field test and the algorithms mentioned here are far from optimal.
Before continuing, remember to update local GAAS and copy related files to corresponding folder:
I mentioned in the last tutorial that it is not possible to measure absolute scale with only one camera without the help of other sensors. RGBD cameras are prone to sunlight, noises and they are expensive，so we will be using stereo camera as the sensor for distance estimation (for mathematical background, check Reading Materials [1]).
A C++ based package, based on StereoBM and WLS disparity filter can be found in GAAS/software/Obstacle_Map. We will not go into details, but the workflow can be organized as:
disparity map generation with opencv provided StereoBM method;
map disparity map to 3D points in camera frame;
map 3D points in camera frame to drone body frame.
Before the build:
To build it:
After building it, in order to visualize how it works, let's first launch the simulation by:
Gazebo window will pop out and you can see a brick wall of size 3x3 meters located before the drone, which is the obstacle you want to avoid while flying to the target defined as 70 meters in front of the drone.
While running Gazebo, if everything works properly with the exception of the above Err message, the Err message itself DOES NOT affect the performance of Gazebo.
If you happen to know how to resolve this Err message, please submit a PR on GitHub to help us make it go away.
Next, you can use:
to generate disparity map and then use:
to convert point cloud in camera frame to drone body frame. You can visualize the result in RVIZ by selecting "add"->"by topic"->"/cloud_in" in RVIZ.
You can see a dense point cloud appeared, although not a perfect square as the brick wall itself. You can modify parameters found in config.yaml to tune the performance and try to find a better result, some of which are:
Next, we can move on to Octomap.
Octomap is a Octree based package to model the environment, and it can drastically decrees the number of points you need to save for memory efficiency and real time performance. We will use Octomap to represent the environment. For more information, check Reading Materials [2] and [3].
To use Octomap:
Before launching Octomap, use rosed to modify resolution to 0.1, max range to 10, remap from="cloud_in" to="cloud_in" and change frame_id to "map":
so your launch file looks like:
Next, start point cloud generation depicted in the last section and launch Octomap by:
in RVIZ, choose to visualize the following topic:
and the corresponding Octomap representation of the environment can be shown as:
You can tune parameters such as Octomap resolution (the size of each cell) and Max range by:
Next, we will talk about how to find a path with Navigator.
Navigator provides a 3D A* path finding algorithm for path finding, it has a poor performance in 3D space, but we will use it as a good starting point. For more information, check Reading Material [4].
In this part, you will need to keep the first two parts up and running and don' t forget to set the following two topics in RVIZ so you can visualize drone current pose and the path found:
Navigator is located at:
to use it, you will need to specify a target position in Navigator.py and current target position it set to 70 meters in front of the drone starting point. After setting up the starting point, you will first take off the drone using:
Next, starting path finding by:
A red flight path will appear and the drone will fly according to the flight path.
Another thing we need to take a look at is path pruning. The path A* generated is a series of way points with incremental step size, and usually the path would be too complex, meaning there are too many extra and unnecessary waypoints. We implemented two path pruning methods:
collinearity check: for a path from A to B, if any two points of it are collinear, remove extra ones and keep only the first and last one;
bresenham3D obstacle detection: for a path from A to B, create lines between A and A+1, A and A+2, check if each line intersect with obstacle, if yes, keep the last point and start overt.
You can visualize the result by:
In the image, the red cube represents an obstacle, the bottom red path is the raw path, consisted of many redundant way points; the green line is the path generated by collinearty check, only corner points are preserved; the blue line is generated by bresenham3D obstacle check, points are preserved according to a preset obstacle distance.
After path pruning, a more efficient and concise path is generated and the drone is able to flly from A to B more efficiently.
If you find any bug or need more help, please let us know either by opening an issue or through our facebook group.
Help us to understand what you want to learn in the upcoming tutorials by fill out this three-question survey.
Give us a Star on Github if you find this tutorial useful.
