ArduPilot Gazebo SITL 3D mapping and planning - View it on GitHub