VIO tracking camera for Non-GPS Navigation¶
This article explains how a VIO tracking camera such as the Intel Realsense T265 can be used with ArduPilot as a substitude for a GPS allowing position control modes like Loiter, PosHold, RTL, Auto. We will demonstrate how to make a MAVLink bridge between the Intel Realsense T265 and ArduPilot in Python, without the use of ROS.
Depends on what you need from the T265, the companion computer should have USB2 (pose data only) or USB3 (pose + image data). For localization and navigation, we only need to capture pose data, so RPi 3B is sufficient for the task.
In a nutshell, the 6-DOF pose data (position and orientation) and confidence level obtained from the Realsense T265 will be processed by our script and send to ArduPilot through MAVLink. Overall, the script will do the following tasks:
- Obtain 6-DOF pose data and tracking confidence level data using relevant APIs from
pyrealsense2, which is the Python wrapper for
- Perform necessary matrix transformation to align the frames of the Realsense T265 and NED frame as well as other processing steps.
- Pack pose data into MAVLink message VISION_POSITION_ESTIMATE and confidence level data into a dummy message, then send them to ArduPilot at a predetermined frequency so as to not flood the Autopilot with incoming data.
- Automatically set EKF home for simple setup and flying.
The Realsense T265 is supported via librealsense on Windows and Linux. Installation process varies widely for different systems, hence refer to the official github page for instructions for your specific system:
For RPi running Ubuntu, the installation process for
librealsense has been detailed in this wiki. Follow the instructions to install
pyrealsense2. Since we are not using ROS,
realsense-ros is not required.
Connect to the autopilot with a ground station (i.e. Mission Planner) and check that the following parameters are set as shown below:
- AHRS_EKF_TYPE = 2 (the default) to use EKF2 (as of this writing, EKF3 is not supported for handling external navigation data)
- EK2_ENABLE = 1 (the default)
- EK3_ENABLE = 0 (the default)
- GPS_TYPE = 0 to disable the GPS
- EK2_GPS_TYPE = 3 to disable the EKF’s use of the GPS
- EK2_POSNE_M_NSE = 0.1
- EK2_VELD_M_NSE = 0.1
- EK2_VELNE_M_NSE = 0.1
- MAG_ENABLE = 0, COMPASS_USE = 0, COMPASS_USE2 = 0, COMPASS_USE3 = 0 to disable the EKF’s use of the compass and instead rely on the heading from external navigation data.
- Configure serial port to connect to companion computer with
SERIALx_PROTOCOL = 1.
After the parameters are modified, reboot the autopilot.
Python Packages Installation¶
- Establish serial connection: Connect RPi to ArduPilot with MAVLink.
- If the connection between RPi-ArduPilot is established via the UART serial port, also change the setting in /boot/config.txt.
- Install Python3.
- You should be able to run the examples provided by Intel can be found in the folder
~/librealsense/wrappers/python/examplewith Python3 command.
- You should be able to run the examples provided by Intel can be found in the folder
# Update the PYTHONPATH environment variable to add the path to the pyrealsense2 library export PYTHONPATH=$PYTHONPATH:/usr/local/lib cd ~/librealsense/wrappers/python/example # You should see a stream of data coming from the T265. python3 t265_example.py
- Install pip for Python3 (pip3).
- Install Python packages:
# pip install may require sudo, so proceed accordingly pip install pyrealsense2 pip3 install transformations pip3 install dronekit pip3 install apscheduler # Install serial packages for serial connection sudo pip3 install pyserial
- Download the script t265_to_mavlink.py. In case you have downloaded the vision_to_mavros package, it can be found in the script folder.
# Navigate to the location of the scripts cd ~/path/to/the/script/ # Download the script if you haven’t already: wget https://raw.githubusercontent.com/hoangthien94/vision_to_mavros/master/scripts/t265_to_mavlink.py chmod +x t265_to_mavlink.py
How to run¶
- Before the script can be run, the
PYTHONPATHenvironment variable needs to be added with the path to the
pyrealsense2library. Alternatively, copy the build output (
~/librealsense/build/) next to the script. First, run the test script
t265_test_streams.pyto verify installation of
pyrealsense2and the T265 is connected.
# Update the PYTHONPATH environment variable to add the path to the pyrealsense2 library export PYTHONPATH=$PYTHONPATH:/usr/local/lib # Navigate to the location of the scripts cd ~/path/to/the/script/ # Download and run a test script, you should see a short stream of pose data coming from the T265 on the terminal wget https://raw.githubusercontent.com/hoangthien94/vision_to_mavros/master/scripts/t265_test_streams.py chmod +x t265_test_streams.py python3 t265_test_streams.py
- Modify parameters in the
t265_to_mavlink.pyscript for your system configuration. Most importantly, find and change the following parameters in the script:
# Default configurations for connection to the FCU connection_string_default = '/dev/ttyUSB0' connection_baudrate_default = 921600 # Default frequency for pose and confidence messages vision_msg_hz_default = 30 confidence_msg_hz_default = 1 # Transformation to convert different camera orientations to NED convention. Replace camera_orientation_default for your configuration. # 0: Forward, USB port to the right # 1: Downfacing, USB port to the right camera_orientation_default = 0
- The parameters can also be passed as input arguments from the command line. Now let’s run the main script:
# For serial connection: set udev.rules in order to get the USB available; allow permission to serial sudo chmod 666 /dev/ttyUSB0 # When everything is working and all defaults are set: python3 t265_to_mavlink.py
View all available input arguments:
python3 t265_to_mavlink.py --help
Verification before testing¶
- To verify that ArduPilot is receiving
VISION_POSITION_ESTIMATEmessages, on Mission Planner: press
Ctrl+Fand click on “Mavlink Inspector”, you should be able to see data coming in. The confidence level can be viewed in message
- Changes in value of the tracking confidence level can also be notified on Mission Planner’s message panel, HUD and by speech. These notifications will pop up when the system starts and when confidence level changes to a new state, for example from
- To enable speech in Mission Planner: Tab Config/Tuning > Planner > Speech > tick on “Enable speech”.
- If there are some messages constantly displayed on the HUD, you might not be able to see / hear the confidence level notification.
- If telemetry is slow, notification might be dropped. You can still see the latest message in MAVLink Inspector, message
- After power on, ssh into the companion computer, navigate to the script and run:
- Wait until the quadcopter icon appears on the map of Mission Planner.
- Pick-up the vehicle and walk it around, check that the vehicle’s position movements are shown on the map. The trajectory of the vehicle on the map should reflect the real movements without too much distortion or overshoot. Below is an example of walking in a 2m x 2m square.
- During the test, view the confidence level and verify tracking performance. For most applications you should trust the full 6dof pose only in high confidence. If you only need the rotation (3dof), lower confidence poses can be used.
- If the external navigation data is lost for any reason (tracking lost, script is interrupted etc.), reboot the Autopilot.
If you are flying in a confined environment, it might be best to go around the safety perimeter of flying, view the trajectory on the map, then remember not to fly/setup mission beyond that perimeter.
For your first flight:
- Takeoff in Stabilize or Alt-Hold, check that the vehicle is stable.
- Move the vehicle around and observe the position on Mission Planner to see if tracking is stable.
- Switch to Loiter, but always ready to switch back to Stabilize/Alt-Hold if anything goes awry.
- Otherwise, the vehicle should hover stably and able to keep its position.
- Move the vehicle around (translate, rotate) at varying speed, always ready to switch back to Stabilize/Alt-Hold.
If everything works as expected, next time you can arm and takeoff in Loiter mode.
Always confirm that position feedback is running ok before switching to Loiter mode. Also look out for the safety boundary in your environment, i.e. where tracking might get lost due to lack of features, fast or rotation movement.
Indoor and Outdoor Experiments¶
- The visual odometry information will appear in the
VISOdataflash log messages.
- EKF’s visual odometry information will appear in XKFD messages
Autorun at boot¶
The script can be run automatically at boot time.
- Download or create a shell file
t265.sh, modify the path to t265_to_mavlink.py script in this shell file, then make it executable:
wget https://raw.githubusercontent.com/hoangthien94/vision_to_mavros/master/scripts/t265.sh nano t265.sh # In t265.sh, change the path to t265_to_mavlink.py, in my case: # /home/ubuntu/catkin_ws/src/vision_to_mavros/scripts/t265_to_mavlink.py chmod +x /path/to/t265.sh # Run test the shell. The script t265_to_mavlink.py should run as normal ./t265.sh
- Depends on your system, use any method to make the script autorun at boot. In the steps below, we will use
systemdto turn it into a service.
- Let’s create a file
/etc/systemd/system/t265.servicewith the following content. Set your actual username after
User=and set the proper path to your
[Unit] Description=Realsense T265 Service After==multi-user.target StartLimitIntervalSec=0 Conflicts= [Service] User=ubuntu EnvironmentFile= ExecStartPre= ExecStart=/home/ubuntu/catkin_ws/src/vision_to_mavros/scripts/t265.sh Restart=on-failure RestartSec=1 [Install] WantedBy=multi-user.target
- That’s it. We can now start the service and automatically get it to start on boot:
systemctl start t265 systemctl enable t265