This repository provides scripts to be used by onboard computers with Intel RealSense Camera's to provide 3-D obstacle avoidance features with ArduPilot. It also provides a easy to use script that supports grabbing depth camera data from Microsoft AirSim and sending Mavlink messages to ArduPilot SITL for simulation.
The depth array as retrieved from the camera is divided into a 3x3 grid. The smallest depth in each grid is stored and converted into a body-frame NEU XYZ vector. This vector is packed into a mavlink message OBSTACLE_DISTANCE_3D (Supported locally by ArduPilot master ONLY) and sent to Flight Controller.
Currently the Avoidance system in ArduPilot is completely 2-D. This is being upgraded to 3D in the following PR: ArduPilot/ardupilot#15553
Demonstration of flight test with this Script and Intel RealSense D455: https://youtu.be/-6PWB52J3ho
Demonstration of AirSim: https://youtu.be/_G5LD6bPeJs
- Clone the branch of the above mentioned PR.
- Update pymavlink from ArduPilot repo.
- ArduPilot Parameters (reboot after setting):
- PRX_TYPE = 2 (proximity lib will look for mavlink messages)
- AVOID_ENABLE = 3 (enable fence + proximity avoidance)
- x86 based Companion Computer (for compatibility with Intel), Ubuntu 18.04 (otherwise, the following installation instruction might not work),
- Python3 (default with Ubuntu 18.04)
-
pip3 install pyrealsense2
-
pip3 install numpy
-
pip3 install pymavlink
-
pip3 install apscheduler
-
pip3 install pyserial
-
pip3 install numba # Only necessary if you want to optimize the performance. - Require pip3 version >= 19 and llvmlite: pip3 install llvmlite==0.34.0
-
pip3 install opencv-python
-
sudo apt -y install python3-gst-1.0 gir1.2-gst-rtsp-server-1.0 gstreamer1 0-plugins-base gstreamer1.0-plugins-ugly libx264-dev
-
sudo apt install python3-opencv
- Copy Companion_Computer directory onto the Companion Computer. Connect the camera.
- Connect the computer to a flight controller running ArduPilot with Master + the above mentioned PR.
- run script: d4xx_to_mavlink_3D.py --connect "Vehicle connection target string"
- --connect: port/IP to connect to
- --baudrate: Set baudrate of connection
- --obstacle_distance_msg_hz: Rate of sending mavlink message, defaults to 15hz
- --debug_enable: Displays RGB and depth view along with depth inside each grid (only enable if valid monitor is connected)
- --camera_name: Camera name to be connected to. If not specified, any valid camera will be connected to randomly. For eg: type 'D435I' to look for Intel RealSense D435I.
Once the camera is connected, check with Mission Planner if Obstacles are being read.
The AirSim Script shares some of the code from the d4xx_to_mavlink script. The idea remains the same. Depth image is grabbed from AirSim and divided into a 3x3 grid. This is a good way to experiment with depth cameras before moving onto real vehicle.
- Setup SITL: https://ardupilot.org/dev/docs/SITL-setup-landingpage.html
- Setup SITL to work with AirSim: https://ardupilot.org/dev/docs/sitl-with-airsim.html
- pip3 install pymavlink
- pip3 install apscheduler
- pip3 install airsim
- pip3 install numpy
- sudo apt-get install python-PIL
- sudo apt install python3-opencv
Add the following section to settings.json file that can be found in Documents/AirSim
"CameraDefaults": {
"CaptureSettings": [
{
"ImageType": 1,
"Width": 640,
"Height": 480,
"FOV_Degrees": 90,
"AutoExposureSpeed": 100,
"MotionBlurAmount": 0
}
]
},
- Launch SITL: sim_vehicle.py -v ArduCopter -f airsim-copter --console --map
- Launch AirSim: ./Blocks.sh -ResX=640 -ResY=480 -windowed
- Launch Script: python3 air_sim_to_mavlink.py
- --connect: port/IP to connect to
- --baudrate: Set baudrate of connection
- --obstacle_distance_msg_hz: Rate of sending mavlink message, defaults to 15hz