A collection of ROS and non-ROS (Python, cpp) code that converts data from vision-based system (external localization system like fiducial tags, VIO, SLAM, or depth image) to corresponding MAVROS topics or MAVLink messages that can be consumed by a flight control stack to support precise localization and navigation tasks.

The code has been tested and come with instructions to be used with ArduPilot. The main sensor is the Intel Realsense Tracking camera T265.

Installation and setup with ArduPilot:

ROS:

Follow this wiki page: https://ardupilot.org/dev/docs/ros-vio-tracking-camera.html or this blog post

non-ROS:

Follow this wiki page: https://ardupilot.org/copter/docs/common-vio-tracking-camera.html

What's included (the main stuffs):

ROS nodes:

vision_to_mavros_node: Transformation of tf pose data to NED frame for vision-related MAVROS topics. Full explaination and usage with Realsense T265 can be found in this blog post and this one with AprilTags.
t265_fisheye_undistort_node: Undistorts and rectifies fisheye images from the Realsense T265 for other packages to consume. Full explaination and usage can be found in this blog post.

non-ROS scripts:

t265_to_mavlink.py: a more elaborate version of vision_to_mavros_node but in Python and is where most of the newly development are put into.
t265_precland_apriltags.py: using the T265 images for the task of precision landing (while using the pose data at the same time), reported in this blog post.
t265_test_streams.py: test if the T265 is connected and librealsense is working properly, extracted from here.

ROS nodes

`vision_to_mavros_node`

ROS package that listens to /tf, transforms the pose of source_frame_id to target_frame_id, then rotate the frame to match body_frame according to ENU convention with user input roll, pitch, yaw, gamma angles.

How it works

Suppose we have a frame named source_frame_id that is measured in a frame named target_frame_id. Let target_frame_id be the world {W} frame, we want to transform source_frame_id to body {B} frame so that {B} and {W} conform to ENU convention (x is pointing to East direction, y is pointing to the North and z is pointing up).

Now assume we already have a default {B} and {W} that are correct in ENU. We will rotate {B} in {W} by an angle gamma_world, in right hand rule. For example, gamma_world equals -1.5707963 (-PI/2) will make {B}'s x axis aligns with {W}'s y axis.
source_frame_id will be aligned with that default {B} by rotating around its own x, y, z axis by angles defined by roll_cam, pitch_cam, yaw_cam, in that order.

Parameters:

target_frame_id: id of target frame (world/map/base_link)
source_frame_id: id of source frame (camera/imu/body_link)
output_rate: the output rate at which the pose data will be published.
roll_cam, pitch_cam, yaw_cam, gamma_world: angles (in radians) that will convert pose received from source_frame_id to body frame, according to ENU conventions.

Subscribed topic:

/tf containing pose/odometry data.

Published topic:

/vision_pose of type geometry_msgs/PoseStamped - single pose to be sent to the FCU autopilot (ArduPilot / PX4), published at a frequency defined by output_rate.
/body_frame/path of type nav_msgs/Path - visualize trajectory of body frame in rviz.

Example applications

Autonomous flight with Intel® RealSense™ Tracking Camera T265 and ArduPilot:

A complete guide including installation, configuration and flight tests can be found by the following blog posts.

There are 3 nodes running in this setup. In 3 separated terminals on RPi:

T265 node: roslaunch realsense2_camera rs_t265.launch. The topic /camera/odom/sample/ and /tf should be published.
MAVROS node: roslaunch mavros apm.launch (with fcu_url and other parameters in apm.launch modified accordingly).

rostopic echo /mavros/state should show that FCU is connected.

rostopic echo /mavros/vision_pose/pose is not published

vision_to_mavros node: roslaunch vision_to_mavros t265_tf_to_mavros.launch

rostopic echo /mavros/vision_pose/pose should now show pose data from the T265.

rostopic hz /mavros/vision_pose/pose should show that the topic is being published at 30Hz.

Once you have verified each node can run successfully, next time you can launch all 3 nodes at once with: roslaunch vision_to_mavros t265_all_nodes.launch, with:

rs_t265.launch as originally provided by realsense-ros.
apm.launch modified with your own configuration.
t265_tf_to_mavros.launch as is.

View trajectory on rviz

After running roslaunch vision_to_mavros t265_all_nodes.launch, here's how to view the trajectory of t265 on rviz:

On host computer, open up rviz: rosrun rviz rviz.
Add Path, topic name: /body_frame/path to rviz.
Change Fixed Frame to target_frame_id, in the case of Realsense T265: camera_odom_frame.

Usage with AprilTag:

roslaunch vision_to_mavros apriltags_to_mavros.launch

This will launch usb_cam to capture raw images, perform rectification through image_proc, use apriltag_ros to obtain the pose of the tag in the camera frame, and finally vision_to_mavros to first get the pose of camera in the tag frame, transform to body frame by using camera orientation, and publish the body pose to /mavros/vision_pose/pose topic. Note that mavros should be launch separately since it has a lot of output on the terminal.

`t265_fisheye_undistort_node`

Image stream from one of the T265’s cameras will be processed to detect AprilTag visual marker, then we will follow MAVLink’s Landing Target Protocol that is supported by ArduPilot to perform precision landing.

non-ROS scripts

`t265_test_streams`

Testing the installation of librealsense and USB connection with the Realsense T265. Extracted from here.

`t265_to_mavlink`

The main Python script that integrates the T265 with ArduPilot. The usage is documented in the following blog posts and wiki pages:

`t265_precland_apriltags`

Same as t265_fisheye_undistort_node, but in Python instead of ROS.

leelaser/vision_to_mavros