To subscribe to the image publisher, subscribe to the vislam/image topic. Currently, the image is outputted at approximately 17Hz 30Hz, but I will be working on trying to speed that up with some optimizations.
This repo provides the sample code and instructions to run Visual-Inertial Simultaneous Localization And Mapping (VISLAM) as a ROS node on the Qualcomm Snapdragon PlatformTM.
This example assumes that you are familiar with ROS framework. If you are new to ROS, refer to ROS Start Guide first to get started.
The current build process is supported only on-target (i.e. on the Snapdragon FlightTM Board). Future release(s) will support off-target cross-compilation on a host computer.
The following is an overall workflow for installation, build and execution of the ROS sample apps for Snapdragon FlightTM
These instructions were tested with version Flight_3.1.2. The latest version of the software can be downloaded from here and installed by following the instructions found here
NOTE: By default the HOME environment variable is not set. Set this up doing the following:
adb shell
chmod +rw /home/linaro
echo "export HOME=/home/linaro/" >> /home/linaro/.bashrc
If you use SSH, the home environment variable should be set correct after the above step. If you use ADB, do the following for each session:
adb shell
source /home/linaro/.bashrc
Get the latest Snapdragon FlightTM qrlSDK for your Ubuntu 14.04 host computer by following the instructions here
NOTE: For this example, you will need the qrlSDK to get the missing files on to the target (see below).
- Platform build setup for camera headers
**NOTE**: For on-target camera development there are few header files missing on the target, but are part of the qrlSDK. To fix this, the missing files need to be pushed on to the target.
This is an interim solution and will be addressed in future releases.
Push the following missing files to the target:
cd <sdk_root_install_path>/sysroots/eagle8074/usr/include
adb push camera.h /usr/include
adb push camera_parameters.h /usr/include
adb shell sync
This installation step requires the Snapdragon FlightTM board to be in station mode with internet connection. See instructions here to set the board in station mode and connect it to your WiFi network.
NOTE Due to an unmet package dependency for fastcv-internal on target, the installation of any deb package using apt-get will fail. To resolve this, do the following:
adb shell
apt-get install -f
This will prompt you to uninstall the fastcv-internal package. Type "Y" to uninstall and continue.
- Install the base ROS package on the target.
The following steps are mirrored from [here](http://wiki.ros.org/indigo/Installation/UbuntuARM). Refer to the link for detailed information about each step.
1. Set your locale
```
adb shell
sudo update-locale LANG=C LANGUAGE=C LC_ALL=C LC_MESSAGES=POSIX
```
1. Setup your sources.list
```
adb shell
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
```
1. Setup your keys
```
adb shell
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
```
1. Installation
```
adb shell
sudo apt-get update
sudo apt-get install ros-indigo-ros-base
```
**NOTE**: If the installation fails with this message:
```
E: Failed to fetch http://ports.ubuntu.com/ubuntu-ports/pool/main/i/icu/icu-devtools_52.1-3_armhf.deb 502 cannotconnect [IP: 91.189.88.150 80]
```
try running the following command:
```
apt-get update --fix-missing
apt-get install ros-indigo-ros-base
```
1. Environment Setup
```
adb shell
echo "source /opt/ros/indigo/setup.bash" >> /home/linaro/.bashrc
```
- Install additional ROS packages on target
adb shell
apt-get install ros-indigo-tf2-ros
After installing ROS, the OpenCL library gets installed by ROS as well which causes a conflict with Machine Vision SDK. To fix this, do the following:
adb shell
dpkg -r libhwloc-plugins
dpkg -r ocl-icd-libopencl1:armhf
NOTE: OpenCL can be safely removed as there is a version that is installed on the target at /usr/lib.
- Download the Snapdragon Machine Vision SDK from here
- The package name will be mv<version>.deb.
** Example: mv0.8.deb - push the deb package to the target and install it.
adb push mv<version>.deb /home/linaro
adb shell sync
adb shell
dpkg -i /home/linaro/mv<version>.deb
echo "export MV_SDK=/opt/qualcomm/mv/lib/mv" >> /home/linaro/.bashrc
The Machine Vision SDK will need a license file to run. Obtain a research and development license file from here
The license file needs to be placed at the same location as the MV SDK library libmv.so.
Push the license file to the target using the following command:
adb push snapdragon-flight-license.bin /opt/qualcomm/mv/lib/mv/bin/lin/8x74/
adb shell sync
adb shell
source /home/linaro/.bashrc
mkdir -p /home/linaro/ros_ws/src
cd /home/linaro/ros_ws/src
catkin_init_workspace
cd ../
catkin_make
echo "source /home/linaro/ros_ws/devel/setup.bash" >> /home/linaro/.bashrc
This ensures that the ROS workspace is setup correctly.
- The repo may be cloned from [here]( from https://github.com/ATLFlight/ros-examples.git) directly on the target, or cloned on the host computer and then pushed to the target using ADB. The recommended method is to clone directly on the target.
adb shell
source /home/linaro/.bashrc
roscd
cd ../src
git clone https://github.com/ATLFlight/ros-examples.git snap_ros_examples
- Build the code
adb shell
source /home/linaro/.bashrc
roscd
cd ..
catkin_make snap_vislam_node
NOTE: To clean the code, remove the "build" folder
adb shell
source /home/linaro/.bashrc
roscd
cd ../
rm -rf build
This example assumes that the user is familiar with ROS framework. If you are new to ROS, refer to ROS Start Guide first to get started.
This assumes that the ROS build command is successful.
NOTE: This process is for running VISLAM in standalone mode only. Integration with a flight stack such as PX4 is not verified.
NOTE: The steps below show each application to be run in a separate adb shell. This is for illustration purpose only. The applications, like rocore, imu_app, snap_vislam_node can be run in the background to meet your development/runtime workflow.
Start roscore in a shell as follows:
adb shell
source /home/linaro/.bashrc
roscore
Starting the imu_app is MANDATORY. Without this, the sample code will not run and the behavior is unpredictable. imu_app is already pre-installed on the target.
For VISLAM to work, it needs the IMU data. On Snapdragon FlightTM, the MPU9x50(IMU) is connected to the ADSP and the Snapdragon FlightTM supports an Apps processor API to get the IMU Data from ADSP. Start the imu_app before using the API to retrieve IMU data.
NOTE: Staring the imu_app will initialize the MPU9x50 driver on ADSP. There can be only one app that initilizes the MPU9x50 driver on ADSP. If there are more than one app that intializes the MPU9x50 driver, the attempts after the first one, will fail.
Run imu_app in a shell.
adb shell
imu_app -s 2
To verify that it works, run the following command in a different shell instance (optional):
adb shell
sensor_imu_tester 5
The app runs for 5 seconds and collects the IMU data received from the ADSP. It would generate a few IMU_*.txt files. If these files get generated and they contain IMU samples, then the IMU data reception mechanism on apps processor works fine.
To start the "snap_vislam_node" ROS node, the 2 steps above: start roscore and start imu_app should be completed to make this work.
adb shell
source /home/linaro/.bashrc
rosrun snap_ros_examples snap_vislam_node
If the above steps are complete and the applications starts successfully, the snap_vislam_node will publish /vislam/pose and /vislam/odometry topics. To view these topics, use the rostopic echo command.
adb shell
source /home/linaro/.bashrc
rostopic echo /vislam/pose
VISLAM requires a minimum set of points to initiaize correctly. If the points are not detected, the pose quality reported by the algorithm is "FAIL" or "INITIALIZING". In this case, there won't be any ROS vislam/pose topics published. One scenario is if board is laying on a surface and the downward camera is very close to the surface.
To fix this, lift up the board a few inches from the surface and set it stationary for few seconds until the initalization is complete. Once completed, you should see the ROS vislam/pose topics published.
The VISLAM algorithm has a parameter, "noInitWhenMoving", that allows to enable or disable initialization when moving. The current example sets this parameter to be true, and hence you need to keep the board stationary. To change the parameter edit the SnapdragonRosNodeVislam.cpp file as follows:
vislamParams.noInitWhenMoving = false;
Once the change is done, recompile the code and re-test.
NOTE: Allowing for initialization when board is moving will potentially give bad VISLAM pose information. The recommended setting is as used in the example.
The example demonstrates console echo for vislam/pose information. To validate if the vislam/pose is correct, you can move the board exercising all 6DOF motion to see the x,y,z changes in both the linear and rotational axes. For example, if you move the board up and down, the "z" axes of the linear pose should change. Similarily if you rotate the board on the "z" axes, the "z" value for the quaternion form should change. See the ROS pose message for the details on the pose message fields.
Since the example is generating a ROS topic, ROS visulation tool like RVIZ can be used to view the pose information. The RVIZ integration is left as an exercise for the user.